def run(self):
""" Runs simulations by quering the robot """
self.reset()
# Initialize robot object
robot = Robot(self.speed, self.turning_speed, self.gps_delay, self.sonar_time, self.tick_move, self.tick_rotate)
robot.set(self.init_position[0], self.init_position[1], self.init_position[2])
robot.set_noise(self.steering_noise, self.distance_noise, self.measurement_noise, self.sonar_noise)
# Initialize robot controller object given by contestant
robot_controller = PythonTimedRobotController(self.robot_controller)
robot_controller.init(
self.init_position,
robot.steering_noise,
robot.distance_noise,
robot.sonar_noise,
robot.measurement_noise,
self.speed,
self.turning_speed,
self.gps_delay,
self.execution_cpu_time_limit,
)
maximum_timedelta = datetime.timedelta(seconds=self.execution_cpu_time_limit)
self.robot_path.append((robot.x, robot.y))
collision_counter = 0 # We have maximum collision allowed
frame_time_left = self.simulation_dt
frame_count = 0
current_command = None
iteration = 0
communicated_finished = False
try:
while (
not communicated_finished
and not robot.time_elapsed >= self.simulation_time_limit
and not self.terminate_flag
):
# logger.info(robot_controller.time_consumed)
if maximum_timedelta <= robot_controller.time_consumed:
raise KrakrobotException("Robot has exceeded CPU time limit")
if iteration % self.iteration_write_frequency == 0:
logger.info("Iteration {0}, produced {1} frames".format(iteration, frame_count))
logger.info("Elapsed {0}".format(robot.time_elapsed))
logger.info(current_command)
iteration += 1
time.sleep(self.simulation_dt)
if frame_time_left > self.frame_dt and self.visualisation:
### Save frame <=> last command took long ###
if (
len(self.robot_path) == 0
or robot.x != self.robot_path[-1][0]
or robot.y != self.robot_path[-1][1]
):
self.robot_path.append((robot.x, robot.y))
self.sim_frames.put(self._create_sim_data(robot))
frame_count += 1
frame_time_left -= self.frame_dt
if current_command is not None:
### Process current command ###
if current_command[0] == TURN:
robot = robot.turn(1) if current_command[1] > 0 else robot.turn(-1)
if current_command[1] > 1:
current_command = [current_command[0], current_command[1] - 1]
elif current_command[1] < 1:
current_command = [current_command[0], current_command[1] + 1]
else:
current_command = None
frame_time_left += self.tick_rotate / self.turning_speed
elif current_command[0] == MOVE:
robot_proposed = robot.move(1)
if not robot_proposed.check_collision(self.grid):
collision_counter += 1
self.collisions.append((robot_proposed.x, robot_proposed.y))
logger.error("##Collision##")
if collision_counter >= KrakrobotSimulator.COLLISION_THRESHOLD:
raise KrakrobotException(
"The robot has been destroyed by wall. Sorry! We miss WALLE already.."
)
else:
robot = robot_proposed
### Register movement, just do not move the robot
if current_command[1] > 1:
current_command = [current_command[0], current_command[1] - 1]
else:
current_command = None
frame_time_left += self.tick_move / self.speed
else:
### Get current command ###
command = None
try:
command = list(robot_controller.act())
except Exception, e:
logger.error("Robot controller failed with exception " + str(e))
break
# logger.info("Received command "+str(command))
# logger.info("Robot timer "+str(robot.time_elapsed))
if not command or len(command) == 0:
raise KrakrobotException("No command passed, or zero length command passed")
# Dispatch command
if command[0] == SENSE_GPS:
robot_controller.on_sense_gps(*robot.sense_gps())
frame_time_left += self.gps_delay
elif command[0] == WRITE_CONSOLE:
new_line = (
"{'frame': "
+ str(frame_count)
+ ", 'time': "
+ str(robot.time_elapsed)
+ "}:\n"
+ command[1]
)
self.logs.append(new_line)
if self.print_robot:
print new_line
elif command[0] == SENSE_SONAR:
w = robot.sense_sonar(self.grid)
robot_controller.on_sense_sonar(w)
frame_time_left += self.sonar_time
elif command[0] == SENSE_FIELD:
w = robot.sense_field(self.grid)
if w == MAP_GOAL:
print robot.x, " ", robot.y, " ", w
if type(w) is not list:
robot_controller.on_sense_field(w, 0)
else:
robot_controller.on_sense_field(w[0], w[1])
frame_time_left += self.light_sensor_time
elif command[0] == TURN:
if len(command) <= 1 or len(command) > 2:
raise KrakrobotException("Wrong command length")
current_command = command
current_command[1] = int(current_command[1])
# Turn robot
# robot = robot.turn(command[1])
elif command[0] == MOVE:
if len(command) <= 1 or len(command) > 2:
raise KrakrobotException("Wrong command length")
if command[1] < 0:
raise KrakrobotException("Not allowed negative distance")
# Move robot
current_command = command
current_command[1] = int(current_command[1])
elif command[0] == FINISH:
logger.info("Communicated finishing")
communicated_finished = True
else:
raise KrakrobotException("Not received command from act(), or command was incorrect :(")
except Exception, e:
logger.error("Simulation failed with exception " + str(e) + " after " + str(robot.time_elapsed) + " time")
return {
"time_elapsed": robot.time_elapsed,
"goal_achieved": False,
"time_consumed_robot": robot_controller.time_consumed.total_seconds() * 1000,
"error": str(e),
}
