public void sendCommand(iCommand command)
{
try
{
int waitingTime = command.waitingTime();
if (command is Pcmd)
{
LOGGER.Debug(String.Format("putting Pcmd's into queue {0} for time {1}", command, waitingTime));
while (waitingTime > 0)
{ // generate enough Pcmds for the expected waiting time
iCommand newCommand = ((Pcmd)command).clone((waitingTime > maxWaitingTime) ? maxWaitingTime : waitingTime);
commonCommandQueue.Add(newCommand);
waitingTime = waitingTime - maxWaitingTime;
}
}
else
{
LOGGER.Debug(String.Format("putting command into queue {0}", command));
if (command is iCommonCommand)
{
//commonCommandQueue.offer(command);
commonCommandQueue.Add(command);
}
else
{
commandQueue.Add(command);
//commandQueue.offer(command);
}
}
}
catch (Exception e)
{
LOGGER.Info("Could not add " + command + " to a queue. " + e.Message);
}
}
public static int waitingTime(this iCommand cmd)
{
var property = cmd.GetType().GetProperty("waitingTime");
if (property != null)
{
if (cmd.waitingTime() == 0)
{
return(100);
}
else
{
return(cmd.waitingTime());
}
}
else
{
return(100);
}
}
private void runConsumer(BlockingCollection <iCommand> queue, bool heartbeat)
{
LOGGER.Info("Creating a specific command queue consumer...");
new Thread(() =>
{
try
{
using (var sumoSocket = new UdpClient())
{
while (sendCommands)
{
try
{
iCommand command = queue.Take();
if (command != null)
{
int cnt = getNextSequenceNumber(command);
byte[] packet = command.getBytes(cnt);
sumoSocket.Send(packet, packet.Length, SumoRemote);
LOGGER.Info(String.Format("Sending command: {0}", command));
Thread.Sleep(command.waitingTime());
}
else // send "null" PCmd (0,0) = ("go by 0 speed") when queue is empty
{
if (heartbeat)
{
Pcmd nullCommand = Pcmd.pcmd(0, 0, maxWaitingTime);
byte[] nullMovePacket = nullCommand.getBytes(getNextSequenceNumber(nullCommand));
//LOGGER.Debug("empty queue, sending null command '{}' with packet {}", nullCommand, convertAndCutPacket(nullMovePacket, false));
sumoSocket.Send(nullMovePacket, nullMovePacket.Length, SumoRemote);
}
Thread.Sleep(maxWaitingTime);
//MILLISECONDS.sleep(maxWaitingTime);
}
}
catch (Exception e)
{
throw new CommandException("Got interrupted while taking a command", e);
}
}
}
}
catch (IOException)
{
LOGGER.Warn("Error occurred while sending packets to the drone.");
}
}).Start();
}
