void IncomingMessageQueue::deserializeAndNotify(unsigned short cur_pos) {
boost::mutex::scoped_lock lock(_mutex);
Header* header = new Header(_messageQueue[cur_pos].header);
Bitstream* bitstream = new Bitstream(_messageQueue[cur_pos].bitstream);
_lastSeqNumCompleted->at(_lastSeqNumQueueIdx) = header->getSeqNum();
++_lastSeqNumQueueIdx;
if (_lastSeqNumQueueIdx == _lastSeqNumQueueLength) {
_lastSeqNumQueueIdx = 0;
}
Message* msg = NULL;
if (_DEBUG)
cout
<< "IncomingMessageQueue::deserializeAndNotify: bitstream->size() = "
<< bitstream->size() << endl;
msg = MessageParser::parseMessage(header, bitstream);
if (msg != NULL) {
double rxTime = (_messageQueue[cur_pos].endTime
- _messageQueue[cur_pos].startTime) / cv::getTickFrequency();
switch (_messageQueue[cur_pos].header.getMsgType()) {
case MESSAGETYPE_DATA_CTA: {
((DataCTAMsg*) (msg))->setTxTime(rxTime);
break;
}
case MESSAGETYPE_DATA_ATC: {
((DataATCMsg*) (msg))->setTxTime(rxTime);
break;
}
default:
break;
}
_nodeNetworkSystem->telosMessageHandler(msg);
}
//erase the entry from the message queue once notified
if (_DEBUG)
cout << "IncomingMessageQueue::deserializeAndNotify: erasing entry"
<< endl;
_messageQueue.erase(_messageQueue.begin() + cur_pos);
if (_DEBUG)
cout << "IncomingMessageQueue::deserializeAndNotify: entry erased"
<< endl;
}
void test_poke_normal_case(const Bitstream& original,
const Bitstream& changed,
const int exit_code,
const uint64_t value,
const size_t start,
const size_t length)
{
Bitstream max_value(length);
max_value.set();
if (start + length > original.size())
{
assert(original == changed);
assert(exit_code == -1);
}
else if(value > max_value.to_ulong())
{
assert(original == changed);
assert(exit_code == -2);
}
else
{
Bitstream sequence(length, value);
Bitstream cpy = original;
for(size_t i = 0; i < sequence.size(); i++)
{
size_t pos = (cpy.size() - start - length) + i;
cpy[pos] = sequence[i];
}
assert(cpy == changed);
assert(exit_code == 0);
for(size_t i = cpy.size()-start; i < cpy.size(); i++)
assert(original[i] == cpy[i]);
for(size_t i = cpy.size()-start-length; i < cpy.size()-start; i++)
assert(changed[i] == cpy[i]);
for(size_t i = 0; i < cpy.size()-start-length; i++)
assert(original[i] == cpy[i]);
}
}
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;
}