/*
* The provided header is first copied and then deleted
*/
void IncomingMessageQueue::addPacketToQueue(Header* header,
const Bitstream& packetBitstream) {
if (_DEBUG)
cout << "IncomingMessageQueue::addPacketToQueue" << endl;
boost::mutex::scoped_lock lock(_mutex);
unsigned int queueIdx;
if (_DEBUG)
cout << "IncomingMessageQueue::addPacketToQueue: header: " << *header
<< endl;
/*
* If the newly arrived packet sequence number has been already deserialized it is ignored
*/
bool seqNumberAlreadyProcessed = false;
for (unsigned char idx = 0; idx < _lastSeqNumQueueLength; ++idx) {
if (_lastSeqNumCompleted->at(idx) == header->getSeqNum()) {
seqNumberAlreadyProcessed = true;
break;
}
}
if (seqNumberAlreadyProcessed) {
cout << "IncomingMessageQueue::addPacketToQueue: duplicate seq num. Ignoring the packet."
<< endl;
} else {
/*
* Find a queue entry with the same sequence number as the newly arrived packet.
*/
for (queueIdx = 0; queueIdx < _messageQueue.size(); ++queueIdx) {
if (header->getSeqNum()
== _messageQueue[queueIdx].header.getSeqNum()) {
break;
}
}
if (queueIdx == _messageQueue.size()) {
/*
* Create a new queue entry
*/
message_queue_entry newEntry;
newEntry.header = *header;
newEntry.lastPacketIdx = header->getPacketIdx();
newEntry.bitstream.insert(newEntry.bitstream.begin(),
packetBitstream.begin(), packetBitstream.end());
newEntry.startTime = cv::getTickCount();
if (newEntry.lastPacketIdx == 0) {
if (_DEBUG)
cout
<< "IncomingMessageQueue::addPacketToQueue: create a new entry"
<< endl;
_messageQueue.push_back(newEntry);
/* If the total number of packets is */
if (header->getNumPackets() == 1) {
_messageQueue[queueIdx].endTime = cv::getTickCount();
_mutex.unlock();
deserializeAndNotify(_messageQueue.size() - 1);
}
} else {
if (_DEBUG)
cout
<< "IncomingMessageQueue::addPacketToQueue: Lost first packet of the message, dropping this packet "
<< endl;
}
} else {
/*
* Append packet bitstream to the right queue entry
*/
int last_packet_id = _messageQueue[queueIdx].lastPacketIdx;
if (header->getPacketIdx() == last_packet_id + 1) {
/*
* Packets order respected
*/
if (_DEBUG)
cout << "IncomingMessageQueue::addPacketToQueue: adding";
_messageQueue[queueIdx].bitstream.insert(
_messageQueue[queueIdx].bitstream.end(),
packetBitstream.begin(), packetBitstream.end());
if (_DEBUG)
cout << " packet";
_messageQueue[queueIdx].lastPacketIdx++;
if (_DEBUG)
cout << " to the queue" << endl;
if (header->getPacketIdx() == header->getNumPackets() - 1) {
_messageQueue[queueIdx].endTime = cv::getTickCount();
_mutex.unlock();
deserializeAndNotify(queueIdx);
}
} else {
/*
* Packets order wrong
*/
if (_DEBUG)
cout
<< "IncomingMessageQueue::addPacketToQueue: not adding packet to the queue: ";
if (header->getPacketIdx() == last_packet_id) {
if (_DEBUG)
cout << "duplicate packet" << endl;
}
if (header->getPacketIdx() > last_packet_id + 1) {
if (_DEBUG)
cout << "packet out of order" << endl;
}
}
}
}
delete header;
}
void Session::_sendMessageThread() {
if (_DEBUG)
cout << "Session::_sendMessageThread" << endl;
Message* msg;
double time;
while (1) {
_messagesQueue.wait_and_pop(msg);
if (_deleted) {
if (_DEBUG)
cout << "Session::_sendMessageThread: deleted thread" << endl;
break;
}
if (_DEBUG)
cout << "Session::_sendMessageThread: Message: " << *msg << endl;
/*
* If Camera set DataATC and DataCTA txTime from previous measurements
*/
switch (NetworkNode::getMyself()->getType()) {
case NODETYPE_CAMERA: {
switch (msg->getType()) {
case MESSAGETYPE_DATA_ATC: {
if (_DEBUG)
cout
<< "Session::_sendMessageThread: setting ATC send time: "
<< _txTimeATC << endl;
((DataATCMsg*) msg)->setTxTime(_txTimeATC);
break;
}
case MESSAGETYPE_DATA_CTA: {
if (_DEBUG)
cout
<< "Session::_sendMessageThread: setting CTA send time: "
<< _txTimeCTA << endl;
((DataCTAMsg*) msg)->setTxTime(_txTimeCTA);
break;
}
default:
break;
}
break;
}
default:
break;
}
time = cv::getCPUTickCount();
Bitstream* msgBitstream = msg->getBitStream();
if (_DEBUG > 2)
cout << "Session::_sendMessageThread: message serialization time: "
<< (cv::getCPUTickCount() - time) / cv::getTickFrequency()
<< " - message bitstream length: " << msgBitstream->size()
<< " byte" << endl;
Header header(msg->getSrc(), msg->getDst(), msg->getSeqNum(), 1, 0,
msg->getType(), msg->getLinkType(), msgBitstream->size());
bool notifySendEnd = msg->getType() == MESSAGETYPE_DATA_ATC
|| msg->getType() == MESSAGETYPE_DATA_CTA;
delete msg;
time = cv::getCPUTickCount();
Bitstream* headerBitstream = header.serializeForIpPacket();
if (_DEBUG > 2)
cout
<< "Session::_sendMessageThread: IP header serialization time: "
<< (cv::getCPUTickCount() - time) / cv::getTickFrequency()
<< endl;
vector<unsigned char> outputBitstream;
outputBitstream.reserve(headerBitstream->size() + msgBitstream->size());
outputBitstream.insert(outputBitstream.end(), headerBitstream->begin(),
headerBitstream->end());
outputBitstream.insert(outputBitstream.end(), msgBitstream->begin(),
msgBitstream->end());
boost::system::error_code error;
write(*_socket,
boost::asio::buffer(outputBitstream, outputBitstream.size()),
error);
delete msgBitstream;
#ifndef GUI
if (notifySendEnd)
NodeNetworkSystem::messageSent();
#endif
if (error) {
_interface->errorHandler(this, error);
return;
}
}
if (_DEBUG)
cout << "Session::_sendMessageThread: End" << endl;
}