private void SendCommand(CommandWithActions toSend)
        {
            try
            {
                int sequenceNumber;
                lock (this)
                {
                    sequenceNumber = _sequenceNumber;
                    _sequenceNumber++;

                    _responseListeners[sequenceNumber] = toSend;
                }

                byte[] payload = toSend.Command.GetBytes(sequenceNumber);
                var sendTask = _streamSpheroWrapper.SendBytes(payload);
                sendTask.Start();
                sendTask.Wait();
            }
            catch (AggregateException exception)
            {
                toSend.OnError(exception.InnerException);
            }
            catch (Exception exception)
            {
                toSend.OnError(exception);
            }
        }
        public void SendAndReceive(ISpheroCommand command, Action<ISpheroMessage> onSuccess, Action<Exception> onError)
        {
            //DoCommand(command, onSuccess, onError);
            var message = new CommandWithActions(command, onSuccess, onError);

            lock (_commandsToSend)
            {
                _commandsToSend.Enqueue(message);
                if (_commandsToSend.Count > 0)
                {
                    _itemsReadyEvent.Set();
                }
            }
        }