////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Tests large data set creation and merging.
///
////////////////////////////////////////////////////////////////////////////////////
void TestLargeDataSets()
{
Packet payload; // Large data set payload.
Header header; // Message header data.
UShort messageCode = 50; // Random message type.
Packet::List stream; // Multi-packet stream sequence.
Header::List streamHeaders; // Mutli-packet stream sequence headers.
unsigned int payloadSize = 60000; // Payload data size to create.
header.mSourceID(50, 1, 1);
header.mDestinationID(51, 1, 1);
for(unsigned int i = 0; i < payloadSize/UINT_SIZE; i++)
{
payload.Write(i);
}
LargeDataSet::CreateLargeDataSet(header, messageCode, payload, stream, streamHeaders, NULL, 1437, 30);
payload.Destroy();
messageCode = 0;
header = Header();
LargeDataSet::MergeLargeDataSet(header, messageCode, payload, stream, NULL);
unsigned int data;
for(unsigned int i = 0; i < payloadSize/UINT_SIZE; i++)
{
payload.Read(data);
if(data != i)
{
std::cout << "Large Data Sets Error: Data Does Not Match!\n";
return;
}
}
std::random_shuffle(stream.begin(), stream.end());
LargeDataSet dataSet;
for(unsigned int i = 0; i < (unsigned int)stream.size(); i++)
{
Header header;
UShort messageCode;
stream[i].SetReadPos(0);
header.Read(stream[i]);
stream[i].Read(messageCode);
if(dataSet.AddPacket(header, messageCode, stream[i]) == false)
{
std::cout << "Large Data Sets Error: Could Not Collect Stream.\n";
}
}
// Now merge the data.
LargeDataSet::MergeLargeDataSet(header, messageCode, payload, dataSet.mStream, NULL);
for(unsigned int i = 0; i < payloadSize/UINT_SIZE; i++)
{
payload.Read(data);
if(data != i)
{
std::cout << "Large Data Sets Error: Data Does Not Match!\n";
return;
}
}
}
Object * Object::Parse(Context * ctx){
Object * ret = NULL;
Header hdr;
hdr.Read(ctx);
if(hdr.GetFlag() == FLAG_ARRAY){
NativeType type = hdr.GetType();
if(type!=0){
int64_t count = hdr.GetLength() / Native::SizeForNative(type);
Array * array = Array::WithType(type,(uint32_t)count);
dynamic_cast<Object*>(array)->Read(hdr, ctx);
ret = array;
}else{
Array * array = Array::WithCapacity();
dynamic_cast<Object*>(array)->Read(hdr, ctx);
ret = array;
}
}else if(hdr.GetFlag() == FLAG_MAP){
Map * obj = new Map();
dynamic_cast<Object*>(obj)->Read(hdr, ctx);
ret = obj;
}else if(hdr.GetFlag() == FLAG_OBJECT){
Native * obj = new Native();
dynamic_cast<Object*>(obj)->Read(hdr, ctx);
ret = obj;
}
return ret;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Creates a multicast listening socket to try and discover
/// subsystems on the network for dynamic selection of a component
/// ID.
///
/// \param[in] discovered List of discovered components performing Discovery.
/// \param[in] waitTimeMs How long to listen for.
/// \param[in] multicastGroup Multicast group to listen on.
///
/// \return True if was able to listen, false on failure to listen.
///
////////////////////////////////////////////////////////////////////////////////////
bool JUDP::ListenForSubsystems(Address::Set& discovered,
const unsigned int waitTimeMs,
const CxUtils::IP4Address& multicastGroup)
{
discovered.clear();
CxUtils::UdpSharedServer listen;
if(listen.InitializeSocket(JUDP::Port, multicastGroup))
{
Packet packet;
Header header;
UShort messageCode;
CxUtils::IP4Address ip;
Time::Stamp start = Time::GetUtcTimeMs();
while(Time::GetUtcTimeMs() - start < waitTimeMs)
{
if(listen.Recv(packet, ip))
{
packet.SetReadPos(1);
if(header.Read(packet) &&
packet.Read(messageCode))
{
discovered.insert(header.mSourceID);
}
}
}
return true;
}
return false;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Reads transport header and payload data from packet, saving to
/// data members of class.
///
/// \param[in] packet Serialized JAUS packet data to read.
/// \param[in] transportHeader Transport header data in front of general
/// transport header.
///
/// \return Number of bytes read on success, FAILURE on error.
///
////////////////////////////////////////////////////////////////////////////////////
int Message::Read(const Packet& packet,
const Packet* transportHeader)
{
unsigned int readPos = packet.GetReadPos();
Header header;
int total = 0;
if(transportHeader && transportHeader->Length() > 0)
{
packet.SetReadPos(readPos + transportHeader->Length());
total = transportHeader->Length();
}
// Try read message transport header data
if(header.Read(packet) > 0)
{
UShort messageCode;
total += Header::MinSize;
total += packet.Read(messageCode);
// Verify this is a message we can actually read.
if(messageCode == mMessageCode)
{
int payloadSize = 0;
if( (payloadSize = ReadMessageBody(packet)) >= 0)
{
total += payloadSize;
mSourceID = header.mSourceID;
mDestinationID = header.mDestinationID;
mPriority = header.mPriorityFlag;
return total;
}
}
}
// Reset read position on failure.
packet.SetReadPos(readPos);
return FAILURE;
}
void Map::Read(const Header & hdr,Context * ctx){
header = hdr;
uint64_t ref = ctx->Tell();
while( (ctx->Tell() - ref) < header.GetLength()){
Header kh;
kh.Read(ctx);
if(kh.GetType() == TYPE_STRING){
Native * native = new Native();
dynamic_cast<Object*>(native)->Read(kh,ctx);
if(native->GetString().size()>0){
Object * obj = Object::Parse(ctx);
map.insert(std::pair<std::string,Object*>(native->GetString(),obj));
delete native;
}
}else{
throw EXCEPTION_WRONG_TYPE;
}
}
}
/// \see http://tfc.duke.free.fr/old/models/md2.htm
void MD2::ModelData::Load(Stream::IStream& stream)
{
Stream::EndianAwareFilter filter(stream);
Header header = { 0 };
header.Read(filter);
if ((header.ident != c_uiMagicNumber) && (header.version != c_uiVersion))
throw Exception(_T("invalid MD2 signature"), __FILE__, __LINE__);
// initialize member variables
m_iNumFrames = header.num_frames;
m_iNumXyz = header.num_xyz;
m_iNumGlcmds = header.num_glcmds;
// allocate memory
m_vecVertices.resize(header.num_xyz * header.num_frames);
m_vecGlcmds.resize(header.num_glcmds);
m_vecLightNormals.resize(header.num_xyz * header.num_frames);
m_vecMinBounds.resize(header.num_frames);
m_vecMaxBounds.resize(header.num_frames);
/////////////////////////////////////////////
// reading file data
std::vector<char> buffer(header.num_frames * header.framesize);
DWORD dwBytesRead = 0;
// TODO read frame endian-independent
// read frame data...
stream.Seek(header.ofs_frames, Stream::IStream::seekBegin);
stream.Read(&buffer[0], buffer.size(), dwBytesRead);
if (buffer.size() != dwBytesRead)
throw Exception(_T("invalid MD2 content"), __FILE__, __LINE__);
// read opengl commands...
stream.Seek(header.ofs_glcmds, Stream::IStream::seekBegin);
for (size_t i = 0; i < m_vecGlcmds.size(); i++)
m_vecGlcmds[i] = static_cast<int>(filter.Read32LE());
/////////////////////////////////////////////
// vertex array initialization
for (unsigned int numframe = 0; numframe < header.num_frames; numframe++)
{
// get frame struct for this
Frame* frame = reinterpret_cast<Frame*>(&buffer[header.framesize * numframe]);
//ATLTRACE(_T("loading frame %u, [%-16hs]\n"), numframe, frame->name);
Vector3d* ptrverts = &m_vecVertices[header.num_xyz * numframe];
unsigned int* ptrnormals = &m_vecLightNormals[header.num_xyz * numframe];
Vector3d vMinBound, vMaxBound;
for (unsigned int i = 0; i < header.num_xyz; i++)
{
Vector3d& v = ptrverts[i];
v = Vector3d(
(frame->verts[i].v[0] * frame->scale[0]) + frame->translate[0],
(frame->verts[i].v[1] * frame->scale[1]) + frame->translate[1],
(frame->verts[i].v[2] * frame->scale[2]) + frame->translate[2]);
ptrnormals[i] = frame->verts[i].lightnormalindex;
// get min and max bounds
vMinBound.X(std::min(v.X(), vMinBound.X()));
vMinBound.Y(std::min(v.Y(), vMinBound.Y()));
vMinBound.Z(std::min(v.Z(), vMinBound.Z()));
vMaxBound.X(std::max(v.X(), vMaxBound.X()));
vMaxBound.Y(std::max(v.Y(), vMaxBound.Y()));
vMaxBound.Z(std::max(v.Z(), vMaxBound.Z()));
}
m_vecMinBounds[numframe] = vMinBound;
m_vecMaxBounds[numframe] = vMaxBound;
}
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Test method to verify ability to read/write message data from
/// an example wireshark log provided for testing systems for the JAUS
/// Interoperability Challenge hosted at several AUVSI competitions.
///
////////////////////////////////////////////////////////////////////////////////////
void VerifyAgainstLog()
{
Packet::List packets;
Packet::List::iterator packet;
Component component;
component.DiscoveryService()->SetSubsystemIdentification(Subsystem::OCU, "OCP");
component.AddService(new LocalPoseSensor());
component.AddService(new VelocityStateSensor());
component.AddService(new ListManager());
component.AddService(new TestLocalWaypointListDriver());
Packet::LoadWiresharkCapturePacketExport("logs/example-capture.txt", packets);
Address cop(42, 1, 1);
Address sim(6000, 1, 1);
//Packet::LoadWiresharkCapturePacketExport("logs/example_capture-2.txt", packets);
Message::List messages;
Message::List::iterator message;
// Delete UDP header data to get to just the JAUS message.
for(packet = packets.begin(); packet != packets.end();)
{
// Delete UDP header data.
packet->Delete(43);
// Read message header data.
Header jausHeader;
if(jausHeader.Read(*packet))
{
UShort messageCode = 0;
packet->Read(messageCode, Header::PayloadOffset);
Message* jausMessage = component.TransportService()->CreateMessage(messageCode);
if(jausMessage)
{
packet->SetReadPos(0);
if(jausMessage->Read(*packet))
{
messages.push_back(jausMessage);
if(jausMessage->GetMessageCode() == SET_ELEMENT)
{
Element::List::iterator m;
SetElement* setElement = (SetElement *)jausMessage;
for(m = setElement->GetElementList()->begin();
m != setElement->GetElementList()->end();
m++)
{
UShort code = 0;
m->mPayload.SetReadPos(0);
m->mPayload.Read(code);
Message* element = component.TransportService()->CreateMessage(code);
if(element)
{
element->CopyHeaderData(jausMessage);
if(element->ReadMessageBody(m->mPayload))
{
messages.push_back(element);
}
else
{
std::cout << "Failed to Read Message Data [" << std::hex << element->GetMessageCode() << "]:\n";
delete element;
}
}
}
}
}
else
{
std::cout << "Failed to Read Message Data [" << std::hex << jausMessage->GetMessageCode() << "]:\n";
}
}
else
{
std::cout << "Unknown Message Type [" << std::hex << messageCode << "]:\n";
}
}
else
{
std::cout << "Bad Header!\n";
}
packet++;
}
component.Initialize(cop);
message = messages.begin();
for(message = messages.begin();
message != messages.end();
message++)
{
//(*message)->Print();
if((*message)->GetSourceID() == cop)
{
(*message)->SetDestinationID(sim);
component.Send((*message));
}
CxUtils::SleepMs(1);
delete (*message);
}
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Method called whenever a UDP packet is received by the
/// service.
///
/// \param[in] packet JAUS UDP Data to process.
/// \param[in] ipAddress IP Address of source of message.
/// \param[in] sourcePort The source port the data was received on.
///
////////////////////////////////////////////////////////////////////////////////////
void JUDP::ProcessUDP(const Packet& packet,
const CxUtils::IP4Address& ipAddress,
const unsigned short sourcePort)
{
// Check for JUDP Version header.
if(packet.Length() > Header::MinSize + mTransportHeader.Length() && *packet.Ptr() == Version)
{
// Strip the JUDP Version header using a wrapper packet (more efficient).
Packet::Wrapper stripped((unsigned char *)(packet.Ptr() + 1), packet.Length() - 1);
Header header;
stripped->SetReadPos(0);
std::string errorMessage;
if(header.Read(*stripped.GetData()) && header.IsValid(&errorMessage))
{
// Don't process messages from ourselves. This happens
// when a message is sent via Multicast/Broadcast or to another
// component on the same computer via UDP.
if(header.mSourceID == mComponentID)
{
if(sourcePort != mMulticast.GetSourcePort() && sourcePort != Port)
{
Mutex::ScopedLock lock(&mDebugMessagesMutex);
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
std::cout << "JAUS ID DUPLICATED BY " << ipAddress.mString << std::endl;
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
std::cout << "=============================================================\n";
}
return;
}
// Update receive times for clients, and make new
// connections if needed.
UpdateClientConnections(header.mSourceID,
ipAddress,
sourcePort);
// Proces the packet data.
ProcessPacket(*stripped.GetData(), header);
/*
// If this component has reserved the primary JAUS port.
if(udpSocket->GetSourcePort() == Port)
{
UShort messageCode = 0;
stripped->Read(messageCode, Header::PayloadOffset);
// Check for a discovery related message.
if(messageCode == QUERY_IDENTIFICATION)
{
// Share this message to other components
// on this host machine so that they can discover
// new connections too.
if(mInternalDiscoverySocket.IsValid() == false)
{
// Set TTL to 0 so we do not transmit outside the host machine.
mInternalDiscoverySocket.InitializeMulticastSocket(mMulticastIP,
mInternalDiscoveryPort,
0,
false,
0,
true);
}
Packet share;
share.Write(GetComponentID().ToUInt());
share.Write(ipAddress.mData, 4);
share.Write(sourcePort);
share.Write(*stripped.GetData());
mInternalDiscoverySocket.Send(share);
}
}
*/
if(header.mSize < stripped->Size())
{
// Multiple packets are included, we must extract them also.
unsigned int offset = header.mSize;
while(offset < stripped->Size())
{
if(mDebugMessagesFlag)
{
Mutex::ScopedLock lock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString() << "] - Received Multi-Message Packet\n";
}
Packet::Wrapper child((unsigned char *)(stripped->Ptr()), stripped->Size() + offset);
if(header.Read(*child.GetData()) && header.IsValid(&errorMessage))
{
ProcessPacket(*child.GetData(), header);
offset += header.mSize;
}
else
{
break; // Can't read anymore data.
}
}
}
}
else if(mDebugMessagesFlag)
{
Mutex::ScopedLock lock(&mDebugMessagesMutex);
std::cout << "[" << GetServiceID().ToString() << "-" << mComponentID.ToString() << "] - Bad JAUS Header [" << errorMessage << "]\n";
}
}
}