  @Test
  public void testMessageOrder() throws InterruptedException {

    int numberOfMessages = 100;
    final CountDownLatch latch = new CountDownLatch(numberOfMessages);

    MessageDispatcher<std_msgs.Int32> messageDispatcher =
        new MessageDispatcher<std_msgs.Int32>(lazyMessages, executorService);
    messageDispatcher.addListener(new MessageListener<std_msgs.Int32>() {
      private AtomicInteger count = new AtomicInteger();

      @Override
      public void onNewMessage(Int32 message) {
        if (this.count.compareAndSet(message.getData(), message.getData() + 1)) {
          latch.countDown();
        } else {
          fail(String.format("Expected message data not equal to actual data: %d != %d",
              count.get(), message.getData()));
        }
        try {
          // Sleeping allows the queue to fill up a bit by slowing down the
          // consumer.
          Thread.sleep(5);
        } catch (InterruptedException e) {
        }
      }
    }, QUEUE_CAPACITY);
    executorService.execute(messageDispatcher);

    for (int i = 0; i < numberOfMessages; i++) {
      final int count = i;
      std_msgs.Int32 message = messageFactory.newFromType(std_msgs.Int32._TYPE);
      message.setData(count);
      lazyMessages.addLast(new LazyMessage<std_msgs.Int32>(message));
    }

    assertTrue(latch.await(1, TimeUnit.SECONDS));
  }