/*
 * 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;
}
Exemplo n.º 2
0
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;

}