def cleanup(world, max_bots=10, remove_from=5):
    """
    Removes the slowest of the oldest `remove_from` robots from
    the world if there are more than `max_bots`
    :param world:
    :type world: World
    :return:
    """
    if len(world.robots) <= max_bots:
        return

    logger.debug("Automatically removing the slowest of the oldest %d robots..." % remove_from)

    # Get the oldest `num` robots
    robots = sorted(world.robot_list(), key=lambda r: r.age(), reverse=True)[:remove_from]

    # Get the slowest robot
    slowest = sorted(robots, key=lambda r: r.velocity())[0]

    fut, hl = yield From(world.add_highlight(slowest.last_position, (50, 50, 50, 50)))
    yield From(fut)
    yield From(trollius.sleep(1))

    # Delete it from the world
    fut = yield From(world.delete_robot(slowest))
    yield From(trollius.sleep(1))
    yield From(remove_highlights(world, [hl]))
    yield From(fut)

	def readCamera(self):
		cap = cv2.VideoCapture(0)
		frame = None
		success = False

		if not cap.isOpened():
			print("Failed to open camera!")
			return

		while True:
			try: 
				success, frame = cap.read()

				if not success:
					print("cap.read() failed")
					yield trollius.From(trollius.sleep(1.0/self.fps))
					continue
				
				self.broadcast(frame)

				if self.hasFrame:
					self.hasFrame(frame)

			except KeyboardInterrupt:
				self.loop.stop()

			except:
				exc_type, exc_value, exc_traceback = sys.exc_info()
				traceback.print_tb(exc_traceback, limit=1, file=sys.stdout)
				traceback.print_exception(exc_type, exc_value, exc_traceback,
	                          limit=2, file=sys.stdout)

			yield trollius.From(trollius.sleep(1.0/self.fps))

		cap.release()

    def publish_loop(self):
        manager = yield From(pygazebo.connect())

        def callback_pose(data):
            self.sensor_comp.callback(data)

        publisher = yield From(
            manager.advertise('/gazebo/default/turtlebot/joint_cmd',
                              'gazebo.msgs.JointCmd'))

        subscriber_pose = manager.subscribe(
            '/gazebo/default/pose/info',
            'gazebo.msgs.PosesStamped',
            callback_pose)
        yield From(subscriber_pose.wait_for_connection())

        msg_left_wheel = pygazebo.msg.joint_cmd_pb2.JointCmd()
        msg_left_wheel.name = 'turtlebot::create::left_wheel'
        msg_right_wheel = pygazebo.msg.joint_cmd_pb2.JointCmd()
        msg_right_wheel.name = 'turtlebot::create::right_wheel'

        print "wait for connection.."
        yield From(trollius.sleep(1.0))

        while True:
            self.brica_agent.step()
            force = self.action_comp.get_result("out_force")

            max_force_strength = 0.7
            force = np.clip(force, -max_force_strength, max_force_strength)

            msg_right_wheel.force, msg_left_wheel.force = force
            From(publisher.publish(msg_left_wheel))
            From(publisher.publish(msg_right_wheel))
            yield From(trollius.sleep(0.00))

def publish_loop(U):
    manager = yield From(pygazebo.connect())

    lift_publisher = yield From(
        manager.advertise('/gazebo/default/trevor/front_gripper/lift_force',
                          'gazebo.msgs.JointCmd'))
    grip_publisher = yield From(
        manager.advertise('/gazebo/default/trevor/front_gripper/grip_force',
                          'gazebo.msgs.JointCmd'))

    lift_message = pygazebo.msg.joint_cmd_pb2.JointCmd()
    lift_message.axis = 0
    lift_message.force = U.lift_force
    grip_message = pygazebo.msg.joint_cmd_pb2.JointCmd()
    grip_message.axis = 0
    grip_message.force = U.grip_force
    lift_message.name = "trevor::front_gripper::palm_raiser"

    while True:
        yield From(lift_publisher.publish(lift_message))
        yield From(trollius.sleep(0.1))
        grip_message.name = "trevor::front_gripper::left_finger_tip"
        yield From(grip_publisher.publish(grip_message))
        yield From(trollius.sleep(0.1))
        grip_message.name = "trevor::front_gripper::right_finger_tip"
        yield From(grip_publisher.publish(grip_message))
        yield From(trollius.sleep(0.1))
        grip_message.name = "trevor::front_gripper::palm_left_finger"
        yield From(grip_publisher.publish(grip_message))
        yield From(trollius.sleep(0.1))
        grip_message.name = "trevor::front_gripper::palm_right_finger"
        yield From(grip_publisher.publish(grip_message))
        yield From(trollius.sleep(0.1))
        print("Publishing")

def pick_up_cubes(r):
    global ganked_cube
    while True:
        val = get_cube(r)

        if val != Colors.NONE:
            r.drive.stop()            
            yield From(asyncio.sleep(0.1))
            val = get_cube(r)

            if val == OUR_COLOR:
                ganked_cube = time.time()
                r.arms.silo.up()
                log.info('Picked up {} block'.format(Colors.name(val)))
                r.arms.silo.down()
            elif val == THEIR_COLOR:
                ganked_cube = time.time()
                r.drive.stop()
                r.arms.dump.up()
                log.info('Picked up {} block'.format(Colors.name(val)))
                r.arms.dump.down()
            else:
                break
        else:
            break

        yield From(asyncio.sleep(0.05))

 def frame_parser(self, reader, writer):
     # This takes care of the framing.
     last_request_id = 0
     while True:
         # Read the frame header, parse it, read the data.
         # NOTE: The readline() and readexactly() calls will hang
         # if the client doesn't send enough data but doesn't
         # disconnect either.  We add a timeout to each.  (But the
         # timeout should really be implemented by StreamReader.)
         framing_b = yield From(asyncio.wait_for(
             reader.readline(),
             timeout=args.timeout, loop=self.loop))
         if random.random()*100 < args.fail_percent:
             logging.warn('Inserting random failure')
             yield From(asyncio.sleep(args.fail_sleep*random.random(),
                                      loop=self.loop))
             writer.write(b'error random failure\r\n')
             break
         logging.debug('framing_b = %r', framing_b)
         if not framing_b:
             break  # Clean close.
         try:
             frame_keyword, request_id_b, byte_count_b = framing_b.split()
         except ValueError:
             writer.write(b'error unparseable frame\r\n')
             break
         if frame_keyword != b'request':
             writer.write(b'error frame does not start with request\r\n')
             break
         try:
             request_id, byte_count = int(request_id_b), int(byte_count_b)
         except ValueError:
             writer.write(b'error unparsable frame parameters\r\n')
             break
         if request_id != last_request_id + 1 or byte_count < 2:
             writer.write(b'error invalid frame parameters\r\n')
             break
         last_request_id = request_id
         request_b = yield From(asyncio.wait_for(
             reader.readexactly(byte_count),
             timeout=args.timeout, loop=self.loop))
         try:
             request = json.loads(request_b.decode('utf8'))
         except ValueError:
             writer.write(b'error unparsable json\r\n')
             break
         response = self.handle_request(request)  # Not a coroutine.
         if response is None:
             writer.write(b'error unhandlable request\r\n')
             break
         response_b = json.dumps(response).encode('utf8') + b'\r\n'
         byte_count = len(response_b)
         framing_s = 'response {0} {1}\r\n'.format(request_id, byte_count)
         writer.write(framing_s.encode('ascii'))
         yield From(asyncio.sleep(args.resp_sleep*random.random(),
                                  loop=self.loop))
         writer.write(response_b)

def run_server():
    conf = parser.parse_args()
    conf.analyzer_address = None

    world = yield From(World.create(conf))
    yield From(world.pause(True))

    with open("/home/elte/mt/tol/scripts/starfish.yaml", "rb") as f:
        robot_yaml = f.read()

    body_spec = world.builder.body_builder.spec
    brain_spec = world.builder.brain_builder.spec
    bot = yaml_to_robot(body_spec, brain_spec, robot_yaml)

    fname = conf.output_directory+"/revolve_benchmark.csv"
    exists = os.path.exists(fname)
    if exists:
        f = open(fname, 'ab', buffering=1)
    else:
        f = open(fname, 'wb', buffering=1)

    output = csv.writer(f, delimiter=',')

    if not exists:
        output.writerow(['run', 'population_size', 'step_size',
                         'sim_time', 'real_time', 'factor'])

    n_bots = [5, 10, 15, 20, 25, 30, 35, 40, 45, 50]
    sim_time = 5.0
    runs = 20

    yield From(world.pause(False))

    for n in n_bots:
        poses = get_poses(n)
        trees = [Tree.from_body_brain(bot.body, bot.brain, body_spec) for _ in range(n)]

        for i in range(runs):
            yield From(wait_for(world.insert_population(trees, poses)))

            while world.last_time is None:
                yield From(trollius.sleep(0.1))

            sim_before = world.last_time
            before = time.time()

            while float(world.last_time - sim_before) < sim_time:
                yield From(trollius.sleep(0.1))

            sim_diff = float(world.last_time - sim_before)
            diff = time.time() - before

            output.writerow((i, n, conf.world_step_size, sim_diff,
                             diff, sim_diff / diff))

            yield From(wait_for(world.delete_all_robots()))
            yield From(trollius.sleep(0.3))

def run():
    """
    :return:
    """
    conf = parser.parse_args()
    conf.world_step_size = 0.004

    fname = conf.output_directory+"/benchmark.csv"
    exists = os.path.exists(fname)
    if exists:
        f = open(fname, 'ab', buffering=1)
    else:
        f = open(fname, 'wb', buffering=1)

    output = csv.writer(f, delimiter=',')

    if not exists:
        output.writerow(['run', 'population_size', 'step_size',
                         'sim_time', 'real_time', 'factor'])

    world = yield From(OnlineEvoManager.create(conf))
    yield From(wait_for(world.pause(False)))

    population_sizes = [5, 10, 15, 20, 25, 30, 35, 40, 45, 50]
    sim_time = 5
    runs = 20

    for n in population_sizes:
        for i in range(runs):
            trees, bboxes = yield From(world.generate_population(n))
            for tree, bbox in zip(trees, bboxes):
                res = yield From(world.birth(tree, bbox, None))
                yield From(res)
                yield From(trollius.sleep(0.05))

            while world.last_time is None:
                yield From(trollius.sleep(0.1))

            sim_before = world.last_time
            before = time.time()

            while float(world.last_time - sim_before) < sim_time:
                yield From(trollius.sleep(0.1))

            sim_diff = float(world.last_time - sim_before)
            diff = time.time() - before

            output.writerow((i, n, conf.world_step_size, sim_diff,
                             diff, sim_diff / diff))

            yield From(wait_for(world.delete_all_robots()))
            yield From(trollius.sleep(0.3))

    f.close()

def run_server():
    conf = Config(update_rate=25, proposal_threshold=0)

    world = yield From(World.create(conf))
    yield From(world.pause(True))
    yield From(yield_wait(world.build_arena()))

    n_bots = [1, 5, 20, 50]
    radius = 0.4 * conf.arena_size[0]
    n_repeats = 1
    sim_time = 5.0

    _state = [None, -1]

    # World update trigger
    def trigger(_):
        elapsed = float(world.last_time)
        if elapsed >= sim_time:
            _state[0] = time.time() - _state[0]
            _state[1] = elapsed

            # Remove trigger to prevent other incoming messages
            # from overwriting these values.
            world.remove_update_trigger(trigger)
        elif elapsed < 0:
            logger.error("Elapsed time < 0!")

    print("nbots\titer\tsimtime\twctime")
    for n in n_bots:
        poses = get_circle_poses(n, radius)

        for i in range(n_repeats):
            # Generate a starting population from the given poses
            yield From(yield_wait(world.generate_starting_population(poses)))
            yield From(trollius.sleep(0.5))
            _state[0] = time.time()
            _state[1] = -1
            world.add_update_trigger(trigger)
            yield From(world.pause(False))

            while _state[1] < 0:
                # Wait until the simulation time has passed the required
                # number of seconds.
                yield From(trollius.sleep(0.1))

            print("%d\t%d\t%f\t%f" % (n, i, _state[1], _state[0]))
            yield From(world.pause())
            yield From(yield_wait(world.delete_all_robots()))

            # Sleep to process all old messages
            yield From(trollius.sleep(0.5))
            yield From(world.reset())

 def control_loop(self, driver, time_out):
     manager = yield From(pygazebo.connect())
     yield From(trollius.sleep(0.5))
     world_subscriber = manager.subscribe('/gazebo/default/world_stats', 'gazebo.msgs.WorldStatistics', self.world_callback)
     location_subscriber = manager.subscribe('/gazebo/default/pose/info', 'gazebo.msgs.PosesStamped', self.location_callback)
     light_sensor1_subscriber = manager.subscribe('/gazebo/default/husky/camera1/link/camera/image', 'gazebo.msgs.ImageStamped', self.light_1_callback)
     light_sensor2_subscriber = manager.subscribe('/gazebo/default/husky/camera2/link/camera/image', 'gazebo.msgs.ImageStamped', self.light_2_callback)
     laser_subscriber = manager.subscribe('/gazebo/default/husky/hokuyo/link/laser/scan', 'gazebo.msgs.LaserScanStamped', self.laser_callback)
     yield From(trollius.sleep(1))
     world_publisher = yield From(manager.advertise('/gazebo/default/world_control','gazebo.msgs.WorldControl'))
     world_publisher.wait_for_listener()
     wheel_publisher = yield From(manager.advertise('/gazebo/default/husky/joint_cmd', 'gazebo.msgs.JointCmd'))
     wheel_publisher.wait_for_listener()
     
     world_control = WorldControl()
     world_control.pause = True
     world_control.reset.all = True
     yield From(trollius.sleep(0.01))
     yield From(world_publisher.publish(world_control))
     
     left_wheel = JointCmd()
     left_wheel.name = 'husky::front_left_joint'
     right_wheel = JointCmd()
     right_wheel.name = 'husky::front_right_joint'
     left_wheel.velocity.target = 0
     right_wheel.velocity.target = 0
     yield From(trollius.sleep(0.01))
     yield From(wheel_publisher.publish(left_wheel))
     yield From(wheel_publisher.publish(right_wheel))
     
     world_control.pause = False
     yield From(trollius.sleep(0.01))
     yield From(world_publisher.publish(world_control))
     
     yield From(trollius.sleep(0.5))
     start_time = self.sim_time
     end_time = start_time + time_out
     
     print "Starting control loop"
     while (self.sim_time < end_time) and (self.distance_to_goal > 0.5):
         sensor_input = [self.light_sensor1, self.light_sensor2]
         sensor_input += self.collide
         (left,right) = driver(sensor_input)
         left_wheel.velocity.target = left
         right_wheel.velocity.target = right
         yield From(wheel_publisher.publish(left_wheel))
         yield From(wheel_publisher.publish(right_wheel))
         self.update_distance()
         if self.distance_to_goal < 0.5:
             break
         yield From(trollius.sleep(.01))
     
     yield From(trollius.sleep(0.01))
     world_control.pause = True
     yield From(world_publisher.publish(world_control))
     
     self.left_velocity = left_wheel.velocity.target
     self.right_velocity = right_wheel.velocity.target
     self.loop.stop()

def publish_loop():
    manager = yield From(pygazebo.connect())

    publisher = yield From(
        manager.advertise('/gazebo/default/simple_arm_0/joint_cmd',
                          'gazebo.msgs.JointCmd'))

    message = pygazebo.msg.joint_cmd_pb2.JointCmd()
    message.name = 'simple_arm_0::arm_shoulder_pan_joint'
    message.position.target = 3

    yield From(publisher.publish(message))
    yield From(trollius.sleep(0.5))
    yield From(publisher.publish(message))
    yield From(trollius.sleep(0.5))

def publish_loop():
    manager = yield From(pygazebo.connect())

    print("have manager")
    publisher = yield From(
        manager.advertise('/gazebo/default/stompy/joint_cmd',
                          'gazebo.msgs.JointCmd'))
    print("have publisher")

    message = pygazebo.msg.joint_cmd_pb2.JointCmd()
    #message.name = 'stompy::fl_leg::thigh_to_calf_upper'
    #message.axis = 1
    #message.force = -1000.0

    message.name = 'stompy::body_to_fl_leg'
    message.axis = 2
    message.force = 1000
    #message.position.p_gain = 10.0
    #message.position.i_gain = 0.5
    #message.position.target = 1.5707

    #message.name = 'stompy::fl_leg::hip_to_thigh'
    #message.axis = 1
    #message.force = -1000.0
    print("have message")

    while True:
        yield From(publisher.publish(message))
        print("published")
        yield From(trollius.sleep(1.0))
        print("sleeping")

 def _wait_data_writer(self):
     for dummy in range(50):
         if not self.data_writer:
             yield From(trollius.sleep(0.1))
         else:
             return
     raise Exception('Time out')

def run_server():
    conf = Config(
        proposal_threshold=0,
        output_directory='output'
    )

    world = yield From(World.create(conf))
    yield From(world.pause(True))

    wall_x = conf.arena_size[0] / 2.0
    wall_y = conf.arena_size[1] / 2.0
    wall_points = [Vector3(-wall_x, -wall_y, 0), Vector3(wall_x, -wall_y, 0),
                   Vector3(wall_x, wall_y, 0), Vector3(-wall_x, wall_y, 0)]

    future = yield From(world.build_walls(wall_points))
    yield From(future)

    grid_size = (1, 2)
    spacing = 3 * conf.mating_distance
    grid_x, grid_y = grid_size
    x_offset = -(grid_x - 1) * spacing * 0.5
    y_offset = -(grid_y - 1) * spacing * 0.5
    poses = [Pose(position=Vector3(x_offset + spacing * i, y_offset + spacing * j, 0))
             for i, j in itertools.product(range(grid_x), range(grid_y))]

    trees, bboxes = yield From(world.generate_population(len(poses)))
    for pose, bbox in itertools.izip(poses, bboxes):
        pose.position += Vector3(0, 0, bbox[2])

    fut = yield From(world.insert_population(trees, poses))
    yield From(fut)

    while True:
        yield From(trollius.sleep(0.1))
        yield From(world.perform_reproduction())

def display_date(loop):
    end_time = loop.time() + 10
    while True:
        print datetime.datetime.now()
        if (loop.time() + 1.0) >=end_time:
            break
        yield From(asyncio.sleep(1))

	def poll(self):
		print("WSResourceServer: poll()")

		rs = {}

		while True:
			try:
				rs = {}
				rs["resources"] = []

				for resource in self.resources:

					r = {}
					r["resourceName"] = resource.name
					r["variables"] = resource.variables
					r["lastUpdated"] = str(datetime.now())

					rs['resources'].append(r)

				self.broadcast(json.dumps(rs))

			except:
				exc_type, exc_value, exc_traceback = sys.exc_info()
				traceback.print_tb(exc_traceback, limit=2, file=sys.stdout)
				traceback.print_exception(exc_type, exc_value, exc_traceback,
                	          limit=6, file=sys.stdout)

			yield asyncio.From(asyncio.sleep(self.pollRate))

 def _set_inferior_tty():
     if self.proc_inftty:
         if self.proc_inftty.returncode is None:
             self.proc_inftty.terminate()
         self.proc_inftty = None
     try:
         self.proc_inftty = proc = yield asyncio.From(
                                 asyncio.create_subprocess_exec(*args))
         info('inferiortty: {}'.format(args))
     except OSError as e:
         self.console_print('Cannot spawn terminal: {}\n'.format(e))
     else:
         start = time.time()
         while time.time() - start < 2:
             try:
                 with open(result_file.name) as f:
                     lines = f.readlines()
                     # Commands found in the result file.
                     if len(lines) == 2:
                         set_inferior_tty_cb(lines[0])
                         set_inferior_tty_cb(lines[1])
                         break
             except IOError as e:
                 self.console_print(
                     'Cannot set the inferior tty: {}\n'.format(e))
                 proc.terminate()
                 break
             yield asyncio.From(asyncio.sleep(.100, loop=self.vim.loop))
         else:
             self.console_print('Failed to start inferior_tty.py.\n')
             proc.terminate()

def rainbow():

	delay = 0.01

	offset = 0

	while True:
		if offset > leds.ledArraySize:
			offset = 0

		x = offset

		for led in range(leds.ledArraySize):

			if x >= leds.ledArraySize:
				x = 0

			wavelength = mp(led, 0, leds.ledArraySize, 780, 380)
			color = wavelengthToRGB(wavelength)
			leds.changeColor(x, color)

			#print("x={0}, offset={1}, color={2}".format(x, offset, color))

			x += 1

		offset += 1

		yield asyncio.From(asyncio.sleep(delay))

 def activity(self):
     backoff = 0
     while True:
         try:
             self.reader, self.writer = yield From(asyncio.open_connection(
                 self.host, self.port, ssl=self.sslctx, loop=self.loop))
         except Exception as exc:
             backoff = min(args.max_backoff, backoff + (backoff//2) + 1)
             logging.info('Error connecting: %r; sleep %s', exc, backoff)
             yield From(asyncio.sleep(backoff, loop=self.loop))
             continue
         backoff = 0
         self.next_id = 0
         self.pending = {}
         self. initialized = True
         try:
             while self.todo:
                 payload, waiter = self.todo.pop()
                 if not waiter.done():
                     yield From(self.send(payload, waiter))
             while True:
                 resp_id, resp = yield From(self.process())
                 if resp_id in self.pending:
                     payload, waiter = self.pending.pop(resp_id)
                     if not waiter.done():
                         waiter.set_result(resp)
         except Exception as exc:
             self.initialized = False
             self.writer.close()
             while self.pending:
                 req_id, pair = self.pending.popitem()
                 payload, waiter = pair
                 if not waiter.done():
                     self.todo.add(pair)
             logging.info('Error processing: %r', exc)

    def _check_resource_monitor(self):
        if not self._resource_monitor:
            return

        for counter in itertools.count():
            resource_info = self._resource_monitor.check()

            if not resource_info:
                if counter:
                    _logger.info(_('Situation cleared.'))
                break

            if counter % 15 == 0:
                if resource_info.path:
                    _logger.warning(__(
                        _('Low disk space on {path} ({size} free).'),
                        path=resource_info.path,
                        size=wpull.string.format_size(resource_info.free)
                    ))
                else:
                    _logger.warning(__(
                        _('Low memory ({size} free).'),
                        size=wpull.string.format_size(resource_info.free)
                    ))

                _logger.warning(_('Waiting for operator to clear situation.'))

            yield From(trollius.sleep(60))