#ifdef WIN32
#pragma warning(disable: 4251)
#pragma warning(disable: 4786)
#pragma warning(disable: 4290)
#include <winsock2.h>
#include <process.h>
#else
#include <unistd.h>
#include <stdlib.h>
#include <sched.h>
#include <netdb.h>
#endif
#include <stdio.h>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <sstream>
#include <iterator>
#include "RTI.hh"
#include "fedtime.hh"
using namespace std;
bool parseCmdLine( int argc, char* argv[] );
string theClassName = "BaseEntity";
string fireInteractionName = "WeaponFire";
string detonateInteractionName = "MunitionDetonation";
bool Master(false);
int numFederates(0);
bool dedicatedMachine(false);
bool unManagedFederate(false);
int sleepTime(850);
string fedFileName("MAKsimpletime.fed");
string federationName("MAKsimpletime");
string initSyncPointLabel("begin");
double theLookAhead = 1.0;
double theTimeAdvance = 5.0;
enum TimeAdvanceService
{
tasUnknown,
tasTimeAdvanceRequest,
tasNextEventRequest,
tasFlushQueue
};
TimeAdvanceService theTimeAdvanceService = tasTimeAdvanceRequest;
bool theAdvanceUsesAvailable = false;
void createFedEx(RTI::RTIambassador* rtiAmb,
string const& fedName,
string const& fedFile)
{
std::cout << "createFederationExecution " << fedName.c_str() << " " << fedFile.c_str() << endl;
try
{
rtiAmb->createFederationExecution(fedName.c_str(), fedFile.c_str());
}
catch(RTI::FederationExecutionAlreadyExists& ex)
{
std::cout << "Could not create Federation Execution: "
<< "FederationExecutionAlreadyExists: "
<< ex._name << " "
<< ex._reason << endl;
}
catch(RTI::Exception& ex)
{
std::cout << "Could not create Federation Execution: " << endl
<< "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl;
exit(0);
}
rtiAmb->tick(0.1, 0.2);
std::cout << "Federation Created" << endl;
}
void joinFedEx(
string const& federateType)
{
bool joined=false;
const int maxTry = 10;
int numTries = 0;
std::cout << "joinFederationExecution " << federateType.c_str() << " " << federationName.c_str() << endl;
while (!joined && numTries++ < maxTry)
{
try
{
rtiAmb->joinFederationExecution(federateType.c_str(), federationName.c_str(), fedAmb);
joined = true;
}
catch(RTI::FederationExecutionDoesNotExist)
{
std::cout << "FederationExecutionDoesNotExist, try " << numTries << "out of " << maxTry << endl;
continue;
}
catch(RTI::Exception& ex)
{
std::cout << "RTI Exception: " << ex._name << " " << ex._reason << endl;
return;
}
rtiAmb->tick(0.1, 0.2);
}
if (joined)
{
std::cout << "Joined Federation." << endl;
}
else
{
std::cout << "Giving up." << endl;
rtiAmb->destroyFederationExecution(federationName.c_str());
exit(0);
}
}
void resignAndDestroy( RTI::RTIambassador* rtiAmb )
{
try
{
rtiAmb->destroyFederationExecution(federationName.c_str());
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "During resign and destroy. " << endl;
}
}
bool publishSubscribeAndRegisterObject(RTI::RTIambassador* rtiAmb)
{
theAmbData.ourAttrs.insert(string("AccelerationVector"));
theAmbData.ourAttrs.insert(string("DeadReckoningAlgorithm"));
theAmbData.ourAttrs.insert(string("Orientation"));
theAmbData.ourAttrs.insert(string("WorldLocation"));
theAmbData.ourAttrs.insert(string("VelocityVector"));
theAmbData.ourAttrs.insert(string("DamageState"));
try
{
theClassHandle = rtiAmb->getObjectClassHandle(theClassName.c_str());
theAmbData.objectClassMap[theClassHandle] = theClassName;
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not get object class handle: "
<< theClassName.c_str() << endl;
return false;
}
string attrName;
RTI::AttributeHandleSet* hSet = RTI::AttributeHandleSetFactory::create(theAmbData.ourAttrs.size());
theAmbData.attrValues = RTI::AttributeSetFactory::create(theAmbData.ourAttrs.size());
set<string>::iterator attributeSetIterator = theAmbData.ourAttrs.begin();
set<string>::iterator attributeSetEnd = theAmbData.ourAttrs.end();
string currentAttribute("");
for ( ; attributeSetIterator != attributeSetEnd; ++attributeSetIterator )
{
currentAttribute = *attributeSetIterator;
try
{
retrievedHandle = rtiAmb->getAttributeHandle(currentAttribute.c_str(), theClassHandle);
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not get attribute handle "
<< attrName.c_str() << endl;
return false;
}
hSet->add(retrievedHandle);
theAmbData.attrValues->add(retrievedHandle, currentAttribute.c_str(), currentAttribute.length()+1);
theAmbData.theAttrNameHandleMap.insert( std::make_pair(currentAttribute, retrievedHandle));
}
int cnt=0;
try
{
rtiAmb->publishObjectClass(theClassHandle, *hSet);
rtiAmb->tick(0.1, 0.2);
cnt=1;
rtiAmb->subscribeObjectClassAttributes(theClassHandle, *hSet);
rtiAmb->tick(0.1, 0.2);
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not "
<< (cnt ? "publish" : "subscribe") << endl;
delete hSet;
return false;
}
string objectName("Talk");
try
{
unsigned int objId = abs(getpid());
stringstream pid;
pid << objId;
objectName += pid.str();
theObjectHandle = rtiAmb->registerObjectInstance(theClassHandle, objectName.c_str());
theAmbData.objectInstanceMap[theObjectHandle] = rtiAmb->getObjectInstanceName(theObjectHandle);
rtiAmb->tick(0.1, 0.2);
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not Register Object "
<< objectName.c_str()
<< " with class "
<< theClassName.c_str() << endl;
delete hSet;
return false;
}
std::cout << "Registered object "
<< objectName.c_str()
<< " with class name "
<< theClassName.c_str() << endl;
delete hSet;
return true;
}
bool publishAndSubscribeInteraction(RTI::RTIambassador* rtiAmb)
{
theAmbData.ourParms.insert("EventIdentifier");
theAmbData.ourParms.insert("FiringLocation");
theAmbData.ourParms.insert("FiringObjectIdentifier");
theAmbData.ourParms.insert("MunitionObjectIdentifier");
theAmbData.ourParms.insert("TargetObjectIdentifier");
try
{
fireInteractionHandle = rtiAmb->getInteractionClassHandle(fireInteractionName.c_str());
theAmbData.interactionClassMap[fireInteractionHandle] = fireInteractionName;
detonateInteractionHandle = rtiAmb->getInteractionClassHandle(detonateInteractionName.c_str());
theAmbData.interactionClassMap[detonateInteractionHandle] = detonateInteractionName;
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not get interaction class handle: "
<< theClassName.c_str() << endl;
return false;
}
theAmbData.paramValues = RTI::ParameterSetFactory::create(theAmbData.ourParms.size());
string paramName;
set<string>::const_iterator parameterIter = theAmbData.ourParms.begin();
set<string>::const_iterator parameterEnd = theAmbData.ourParms.end();
for ( ; parameterIter != parameterEnd; ++parameterIter )
{
paramName = *parameterIter;
try
{
retrievedHandle = rtiAmb->getParameterHandle(paramName.c_str(), fireInteractionHandle);
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not get parameter handle "
<< paramName.c_str() << endl;
return false;
}
theAmbData.paramValues->add(retrievedHandle, paramName.c_str(), paramName.length()+1);
theAmbData.theParamNameHandleMap.insert( std::make_pair(paramName, retrievedHandle));
}
int cnt=0;
try
{
rtiAmb->publishInteractionClass(fireInteractionHandle);
rtiAmb->tick(0.1, 0.2);
rtiAmb->publishInteractionClass(detonateInteractionHandle);
rtiAmb->tick(0.1, 0.2);
cnt=1;
rtiAmb->subscribeInteractionClass(fireInteractionHandle);
rtiAmb->tick(0.1, 0.2);
rtiAmb->subscribeInteractionClass(detonateInteractionHandle);
rtiAmb->tick(0.1, 0.2);
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception: "
<< ex._name << " "
<< ex._reason << endl
<< "Could not "
<< (cnt ? "publish" : "subscribe")
<< " to interaction." << endl;
return false;
}
std::cout << "Subscribed to interaction class: "
<< fireInteractionName.c_str()
<< " with handle: "
<< fireInteractionHandle << endl
<< " and interaction class "
<< detonateInteractionName.c_str()
<< " with handle: "
<< detonateInteractionHandle << endl;
return true;
}
void cleanUpProgram(RTI::RTIambassador* rtiAmb)
{
try
{
if ( theAmbData.isRegulating )
{
rtiAmb->disableTimeRegulation();
}
resignAndDestroy(rtiAmb);
delete rtiAmb;
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception (during exit): " << ex._name << " " << ex._reason << endl;
}
#ifdef WIN32
WSACleanup();
#endif
exit(0);
}
void printOutFedTime(RTI::FedTime& time)
{
char* buff = new char[time.getPrintableLength()];
time.getPrintableString(buff);
std::cout << atof(buff) ;
delete [] buff;
}
void sleepFunction()
{
if ( ! dedicatedMachine )
{
#ifdef WIN32
Sleep(sleepTime);
#else
usleep(sleepTime * 1000);
#endif
}
}
void minimalSleepFunction()
{
#ifdef WIN32
Sleep(10);
#else
usleep(10 * 1000);
#endif
}
void yieldFunction()
{
if ( ! dedicatedMachine )
{
#ifdef _WIN32
Sleep( 0 );
#elif __solaris__
yield();
#else
sched_yield();
#endif
}
}
void becomeConstrainedAndRegulating(RTI::RTIambassador* rtiAmb,
RTI::FedTime& currentTime,
RTI::FedTime& lookAhead )
{
try
{
rtiAmb->enableTimeRegulation(currentTime, lookAhead);
std::cout << "Waiting to become Time Regulating \n";
rtiAmb->tick(.1, 1);
while ( ! theAmbData.isRegulating )
{
std::cout << ".";
{
cleanUpProgram(rtiAmb);
}
minimalSleepFunction();
rtiAmb->tick(.1, 1);
}
std::cout << endl;
rtiAmb->enableTimeConstrained();
std::cout << "Waiting to become Time Constrained \n";
rtiAmb->tick(.1, 1);
while ( ! theAmbData.isConstrained )
{
std::cout << ".";
{
cleanUpProgram(rtiAmb);
}
rtiAmb->tick(.1, 1);
if ( ! theAmbData.isConstrained )
{
yieldFunction();
}
}
std::cout << endl;
}
catch (RTI::Exception& )
{
std::cout << " unable to become regulating and constrained\n" << " Exiting ...\n";
cleanUpProgram(rtiAmb);
}
}
void synchronizeFederation(RTI::RTIambassador* rtiAmb)
{
if ( !theAmbData.isMaster )
{
rtiAmb->tick(0.1, 0.2);
std::cout << " Ordinary federate is waiting for synchPoint message\n";
while (!theAmbData.announceSyncReceived)
{
{
cleanUpProgram(rtiAmb);
}
yieldFunction();
rtiAmb->tick(0.1, 0.2);
}
std::cout << "sync Point message received\n";
}
else
{
std::string synchPointTag("");
int maximumSize(256);
char* hostName = new char[maximumSize];
if ( -1 == gethostname(hostName, maximumSize) )
{
unsigned int objId = abs(getpid());
std::stringstream pid;
pid.str(std::string(""));
pid << "unknownHostName" << "_" << objId;
synchPointTag = pid.str();
}
else
{
unsigned int objId = abs(getpid());
std::stringstream pid;
pid.str(std::string(""));
pid << hostName << "_" << objId;
synchPointTag = pid.str();
}
delete hostName;
rtiAmb->tick(0.1, 0.2);
std::cout << " Master waiting for all federates to Join.\n";
while ( theAmbData.otherFederatesReady < (theAmbData.numFederates - 1) )
{
{
cleanUpProgram(rtiAmb);
}
yieldFunction();
rtiAmb->tick(0.1, 0.2);
}
std::cout << " Master federate has discovered the required number of other federates.\n\n";
rtiAmb->registerFederationSynchronizationPoint(initSyncPointLabel.c_str(), synchPointTag.c_str());
rtiAmb->tick(0.1, 0.2);
while ( !theAmbData.registerFailed && !theAmbData.registerSucceeded )
{
{
cleanUpProgram(rtiAmb);
}
yieldFunction();
rtiAmb->tick(0.1, 0.2);
}
if ( theAmbData.registerFailed )
{
std::cout << " We failed to register synch point " << initSyncPointLabel << std::endl
<< " with tag " << synchPointTag << std::endl;
cleanUpProgram(rtiAmb);
}
std::cout << "Master successfully registered\n";
}
try
{
rtiAmb->synchronizationPointAchieved(initSyncPointLabel.c_str());
}
catch (RTI::Exception& )
{
std::cout << " Synch point achieved failed\n" << " Exiting....\n";
cleanUpProgram(rtiAmb);
}
rtiAmb->tick(0.1, 0.2);
while ( !theAmbData.federationIsSynchronized )
{
{
cleanUpProgram(rtiAmb);
}
minimalSleepFunction();
rtiAmb->tick(0.1, 0.2);
}
}
int main(int argc, char** argv)
{
std::cout << "MAK simpleTime version 1.3" << std::endl;
try
{
string federateType("simpletime13");
if (argc > 1)
{
if (!parseCmdLine(argc, argv))
{
exit(0);
}
}
RTI::RTIambassador* rtiAmb = new RTI::RTIambassador;
createFedEx(rtiAmb, federationName, fedFileName);
joinFedEx(rtiAmb, &fedAmb, federateType);
#ifdef WIN32
WSADATA data;
WSAStartup(MAKEWORD(1,1), &data);
#endif
rtiAmb->tick(0.1, 0.2);
long count=0;
RTIfedTime oneTimeStep = RTIfedTime(theTimeAdvance);
RTIfedTime lookAhead = RTIfedTime(theLookAhead);
RTIfedTime updateTime = RTIfedTime(0.0);
RTIfedTime interactionTime = RTIfedTime(0.0);
RTIfedTime epsilon;
theAmbData.time = RTI::FedTimeFactory::makeZero();
RTI::FedTime& currentTime = *theAmbData.time;
epsilon.setEpsilon();
becomeConstrainedAndRegulating(rtiAmb, *theAmbData.time, lookAhead);
if (!publishSubscribeAndRegisterObject(rtiAmb))
{
resignAndDestroy(rtiAmb);
delete rtiAmb;
return 0;
}
if ( !unManagedFederate )
{
synchronizeFederation(rtiAmb);
}
if (!publishAndSubscribeInteraction(rtiAmb))
{
resignAndDestroy(rtiAmb);
delete rtiAmb;
return 0;
}
bool hasDetonated = false;
while (!hasDetonated && !theAmbData.timeManagedObject.isDetonated())
{
try
{
string tag;
if (*theAmbData.time < updateTime)
{
theAmbData.timeManagedObject.tick();
}
else
{
updateTime += oneTimeStep;
std::cout << "===================================================================\n";
std::cout << "Current time (";
printOutFedTime(currentTime);
std::cout << "), Lookahead (";
printOutFedTime(lookAhead);
std::cout << ") update time (";
printOutFedTime(updateTime);
std::cout << ")" << std::endl;
std::cout << endl;
stringstream ss;
ss << "1.3-" << count++;
tag = ss.str();
theAmbData.timeManagedObject.tick();
theAmbData.timeManagedObject.incPosition(theTimeAdvance);
rtiAmb->updateAttributeValues(theObjectHandle, *(theAmbData.attrValues), updateTime, tag.c_str());
rtiAmb->tick(0.001, 0.5);
}
if ( theAmbData.timeManagedObject.shouldDetonate() )
{
string munitionString = "MunitionObjectIdentifier";
RTI::ParameterHandle munitionHandle = theAmbData.theParamNameHandleMap.find(munitionString)->second;
interactionTime = currentTime;
interactionTime += lookAhead;
interactionTime += epsilon;
theAmbData.paramValues->remove( munitionHandle );
theAmbData.paramValues->add(munitionHandle, "\0", 1);
rtiAmb->sendInteraction(detonateInteractionHandle, *theAmbData.paramValues, interactionTime, tag.c_str());
theAmbData.paramValues->remove( munitionHandle );
theAmbData.paramValues->add(munitionHandle,
munitionString.c_str(), munitionString.length() + 1);
hasDetonated = true;
}
if ( theAmbData.timeManagedObject.shouldFire() )
{
string fireString = "FiringObjectIdentifier";
interactionTime = currentTime;
interactionTime += lookAhead;
interactionTime += epsilon;
theAmbData.paramValues->remove(fireHandle);
theAmbData.paramValues->add(fireHandle, "\0", 1);
rtiAmb->sendInteraction(fireInteractionHandle, *theAmbData.paramValues, interactionTime, tag.c_str());
theAmbData.paramValues->remove(fireHandle);
theAmbData.paramValues->add(fireHandle, fireString.c_str(), fireString.length() + 1);
}
}
catch (RTI::InvalidFederationTime& ex)
{
std::cout << " caught an Invalid Federation Time !!\n";
std::cout << ex._name << " " << ex._reason << endl;
cleanUpProgram(rtiAmb);
}
int kb(0);
try
{
if ( theAmbData.isRegulating )
{
if (theTimeAdvanceService == tasTimeAdvanceRequest)
{
if (theAdvanceUsesAvailable)
{
rtiAmb->timeAdvanceRequestAvailable(updateTime);
}
else
{
rtiAmb->timeAdvanceRequest(updateTime);
}
}
else if (theTimeAdvanceService == tasNextEventRequest)
{
if (theAdvanceUsesAvailable)
{
rtiAmb->nextEventRequestAvailable(updateTime);
}
else
{
rtiAmb->nextEventRequest(updateTime);
}
}
else if (theTimeAdvanceService == tasFlushQueue)
{
rtiAmb->flushQueueRequest(updateTime);
}
else
{
rtiAmb->timeAdvanceRequest(updateTime);
}
theAmbData.timeAdvanced = false;
rtiAmb->tick(0.001, 0.2);
while ( ! theAmbData.timeAdvanced )
{
if (kb < 0)
{
break;
}
else if ( kb == 1 )
{
theAmbData.timeManagedObject.firePressed();
std::cout << "********** Fire pressed at time ";
printOutFedTime(updateTime);
std::cout << " *************\n";
}
else if ( !theAmbData.timeAdvanced )
{
yieldFunction();
rtiAmb->tick(0.001, 0.2);
}
}
}
}
catch( RTI::Exception& ex )
{
std::cout << "RTI Exception" << endl
<< ex._name << " " << ex._reason << endl
<< " Could not advance time" << endl;
}
rtiAmb->tick(0.001, 0.2);
if ( kb == 0 )
{
}
if ( kb < 0 )
{
break;
}
else if ( kb == 1 )
{
theAmbData.timeManagedObject.firePressed();
std::cout << "********** Firing at Time ";
printOutFedTime(updateTime);
std::cout << " *************\n";
}
std::cout << "===================================================================\n\n";
sleepFunction();
}
delete theAmbData.time;
delete theAmbData.paramValues;
delete theAmbData.attrValues;
if ( theAmbData.isRegulating )
{
rtiAmb->disableTimeRegulation();
}
resignAndDestroy(rtiAmb);
delete rtiAmb;
}
catch (RTI::Exception& ex)
{
std::cout << "RTI Exception (main loop): " << ex._name << " " << ex._reason << endl;
}
#ifdef WIN32
WSACleanup();
#endif
return 0;
}
bool parseCmdLine(int argc, char* argv[])
{
vector < std::string > cmdArgs;
copy(argv + 1, argv + argc, back_inserter(cmdArgs));
vector<std::string>::const_iterator cur = cmdArgs.begin();
vector<std::string>::const_iterator last = cmdArgs.end();
while (cur != last)
{
vector<std::string>::const_iterator next = cur + 1;
if (*cur == "-h")
{
return false;
}
else if (*cur == "-fedFile")
{
if (next == last || !convert<string>(*cur, *next, fedFileName))
{
return false;
}
++cur;
}
else if (*cur == "-m")
{
if (next == last || !convert<int>(*cur, *next, theAmbData.numFederates))
{
return false;
}
theAmbData.isMaster = true;
++cur;
}
else if (*cur == "-phaseLine")
{
double phaseLine(0);
if (next == last || !convert<double>(*cur, *next, phaseLine))
{
return false;
}
theAmbData.timeManagedObject.setPhaseLine(phaseLine);
++cur;
}
else if (*cur == "-sleepTime")
{
if (next == last || !convert<int>(*cur, *next, sleepTime))
{
return false;
}
++cur;
}
else if (*cur == "-dedicated")
{
dedicatedMachine = true;
}
else if (*cur == "-unManaged")
{
unManagedFederate = true;
}
else if (*cur == "-lookAhead")
{
if (next == last || !convert<double>(*cur, *next, theLookAhead))
{
return false;
}
++cur;
}
else if (*cur == "-timeIncrement")
{
if (next == last || !convert<double>(*cur, *next, theTimeAdvance))
{
return false;
}
++cur;
}
else if (*cur == "-available")
{
theAdvanceUsesAvailable = true;
}
else if (*cur == "-advanceTimeWith")
{
std::string timeAdvanceService;
if (next == last || !convert<string>(*cur, *next, timeAdvanceService))
{
return false;
}
++cur;
if (timeAdvanceService == "timeAdvanceRequest")
{
theTimeAdvanceService = tasTimeAdvanceRequest;
}
else if (timeAdvanceService == "nextMessageRequest")
{
theTimeAdvanceService = tasNextEventRequest;
}
else if (timeAdvanceService == "flushQueue")
{
theTimeAdvanceService = tasFlushQueue;
}
else
{
return false;
}
}
else
{
return false;
}
++cur;
}
return true;
}