int main(int argc, char * argv[])
{
MOOS::MOOSAsyncCommClient C;
MOOS::CommandLineParser P(argc,argv);
C.AddMessageCallback("CallbackA","X",func,NULL);
C.AddMessageCallback("CallbackB","Y",func,NULL);
C.SetOnConnectCallBack(on_connect, &C);
C.Run("localhost",9000,"reg_test");
std::cerr<<"now run \"umm -p=X@2,Y@2\n\"";
while(1)
{
std::cerr<<"waiting 5 seconds\n";
MOOSPause(5000);
std::cerr<<"unregistering for everything\n";
C.UnRegister("*","*");
std::cerr<<"waiting 5 seconds\n";
MOOSPause(5000);
std::cerr<<"registering for \"X\" and \"Y\"\n";
C.Register("X");
C.Register("Y");
}
}
bool CMOOSSerialPort::GetTelegramOrAccumulate(std::string &sTelegram,double dfTimeOut,double *pTime)
{
if(IsStreaming())
{
MOOSTrace("don't call GetTelegram on a streaming device!\n");
return false;
}
static char telegramBuffer[TELEGRAM_LEN];
static int nTelegramBufferRead = 0; //total number of chars read
double dfTimeWaited = 0.0; //haven't waited any time yet
double dfInterval = 0.01; //10ms
while ((dfTimeWaited<dfTimeOut) && nTelegramBufferRead<TELEGRAM_LEN)
{
int nGrabbed = 0;
//try the read
nGrabbed = GrabN(telegramBuffer+nTelegramBufferRead,1);
if (nGrabbed == 0)
{
//OK wait a while...maybe it is on its way!
dfTimeWaited+=dfInterval;
MOOSPause((int)(dfInterval*1000.0));
}
else
{
if(nTelegramBufferRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
nTelegramBufferRead+=nGrabbed;
//have we reached the end of the message?
if(IsCompleteReply(telegramBuffer,TELEGRAM_LEN,nTelegramBufferRead))
{
telegramBuffer[nTelegramBufferRead]='\0';
nTelegramBufferRead = 0;
sTelegram = telegramBuffer;
MOOSRemoveChars(sTelegram,"\r\n");
if(IsVerbose())
{
MOOSTrace("Telegram = %s\n",sTelegram.c_str());
}
//MOOSTrace("Required %d retries and %d accumulates\n",nRetries,nAccumulates);
return true;
}
}
}
return false;
}
int main(int argc, char * argv[])
{
MOOS::CommandLineParser P(argc,argv);
std::string db_host = "localhost";
P.GetVariable("--moos_host",db_host);
int db_port = 9000;
P.GetVariable("--moos_port",db_port);
std::string my_name = "Practice App 03";
P.GetVariable("--moos_name",my_name);
MOOS::MOOSAsyncCommClient Comms;
Comms.SetOnMailCallBack(OnMail,&Comms);
Comms.SetOnConnectCallBack(OnConnect,&Comms);
Comms.Run(db_host,db_port,my_name);
std::vector<unsigned char> X(100);
int Y = 0;
while(1)
{
MOOSPause(1000);
Comms.Notify("X",X);
Comms.Notify("Y",Y++);
}
return 0;
}
bool f(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/)
{
//lets wait a while
MOOSPause(2);
//some stats
double dfE = TimeNow-TimeScheduled;
N=N+1;
dfMean = dfMean*(N-1)/N+(dfE)/N;
if(N>1)
dfVar = dfE/(N-1)+(N-1)/N*dfVar;
std::cout.setf(std::ios::fixed);
std::cout<<std::setprecision(4);
std::cout<<"Timer Callback \n";
std::cout<<" TimeNow "<<TimeNow<<"\n";
std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
std::cout<<" Error "<<(TimeNow-TimeScheduled)*1000.0<<"ms\n";
std::cout<<" Mean Error "<<dfMean*1000.0<<"ms\n";
std::cout<<" Std Error "<<std::sqrt(dfVar)*1000.0<<"ms\n";
return true;
}
int CMOOSSerialPort::ReadNWithTimeOut(char *pData, int nLen, double dfTimeOut,double * pTime )
{
int nSpace = nLen; //space left in buffer
int nRead = 0; //total number of chars read
bool bQuit = false; // exit flag on complete message
double dfStopTime=MOOSLocalTime()+dfTimeOut;
while (MOOSLocalTime()<dfStopTime && !bQuit)
{
//try the read
int nGrabbed = GrabN(pData+nRead,nSpace);
if (nGrabbed == 0)
{
// wait a while...maybe it is on its way!
MOOSPause(10);
}
else if(nGrabbed<0)
{
MOOSTrace("Grab FAILED %s\n",MOOSHERE);
MOOSPause(10);
}
else
{
if(nRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
nSpace-=nGrabbed;
nRead+=nGrabbed;
if(nSpace<=0)
bQuit = true;
}
}
return nRead;
}
int main(int argc, char * argv[])
{
MOOS::MOOSAsyncCommClient C;
MOOS::CommandLineParser P(argc,argv);
if(P.GetFlag("-h","--help"))
{
PrintHelpAndExit();
}
InterestedParty aClass;
//C.AddMessageRouteToActiveQueue("CallbackA","la",func,NULL);
//C.AddMessageRouteToActiveQueue("CallbackB","di",func,NULL);
//C.AddMessageRouteToActiveQueue("CallbackC","di",func_alt,NULL);
//C.AddMessageRouteToActiveQueue("Wildcard","*", func_wild,NULL);
//C.AddMessageRouteToActiveQueue("ClassMember","la", &aClass,&InterestedParty::HandleMessageA);
//C.AddMessageRouteToActiveQueue("ClassMember","di", &aClass,&InterestedParty::HandleMessageB);
//C.AddWildcardActiveQueue("WCA","*", func_wildcard,NULL);
//C.PrintMessageToActiveQueueRouting();
MOOS::MessageQueueAccumulator Acc;
std::vector<std::string> Names;
Names.push_back("di");
Names.push_back("la");
Acc.Configure(Names);
C.AddMessageRouteToActiveQueue("Accumulator","di", &Acc,&MOOS::MessageQueueAccumulator::AddMessage);
C.AddMessageRouteToActiveQueue("Accumulator","la", &Acc,&MOOS::MessageQueueAccumulator::AddMessage);
Acc.SetCallback(&aClass,&InterestedParty::HandleMessageSet);
//C.lala();
C.SetOnConnectCallBack(on_connect, &C);
C.Run("localhost",9000,"queue_test");
unsigned int j = 0;
while(++j)
{
if(j%100==0)
{
C.AddWildcardActiveQueue("WCB","d*", &aClass,&InterestedParty::HandleMessageC);
C.PrintMessageToActiveQueueRouting();
}
MOOSPause(100);
continue;
}
}
bool CMOOSActuationDriver::SendAndAck(const string & sCmd,string &sReply,bool bWait)
{
if(m_pPort==NULL)
return false;
if(m_bVerbose)
{
MOOSTrace("Send: %s\n",sCmd.c_str());
}
m_pPort->Write((char*)sCmd.c_str(),
sCmd.size());
//if we are required to read a reply
if(bWait)
{
if(!m_pPort->GetTelegram(sReply,TELEGRAM_PAUSE))
{
MOOSTrace("no terminated reply to \"%s\" from actuation hardware\n",sCmd.c_str());
return false;
}
}
else
{
//wait fort answer an ignore..
MOOSPause((int)(0.05*TELEGRAM_PAUSE*1000));
//Simply flush...
m_pPort->Flush();
}
if(m_bVerbose)
{
if(bWait)
{
MOOSTrace("Rx: %s\n",sReply.c_str());
}
else
{
MOOSTrace("Rx: No wait requested\n");
}
}
return true;
}
int CMOOSSerialPort::ReadNWithTimeOut2(char *pData, int nLen, double dfTimeOut, double * pTime )
{
int nSpace = nLen; //space left in buffer
int nRead = 0; //total number of chars read
bool bQuit = false; //exit flag on complete message
double dfStopTime=MOOSLocalTime()+dfTimeOut;
while (MOOSLocalTime()<dfStopTime && !bQuit)
{
//try the read
int nGrabbed = GrabN(pData+nRead,nSpace);
if (nGrabbed == 0)
{
// wait a while...maybe it is on its way!
MOOSPause(10);
}
else if(nGrabbed<0)
{
return -1; // Signal error
}
else
{
// Ensures that we only grab the time for the first byte.
if(nRead==0 && pTime!=NULL)
{
//grab the time..
*pTime = MOOSTime();
}
//great, so increment out buffer pointer
//and check to see if this is a complete
// nomad reply
nSpace-=nGrabbed;
nRead+=nGrabbed;
if(nSpace<=0)
bQuit = true;
}
}
return nRead;
}
int main(int argc, char * argv[]){
//configure the comms
MOOS::MOOSAsyncCommClient Comms;
Comms.SetOnMailCallBack(OnMail,&Comms);
Comms.SetOnConnectCallBack(OnConnect,&Comms);
//start the comms running
Comms.Run("localhost",9000,"EX20");
for(;;){
MOOSPause(1000);
Comms.Notify("Greeting","Hello");
}
return 0;
}
int main(int argc , char * argv[])
{
//default command line parameters - Mission file and port number
const char * sMissionFile = "Mission.moos";
if(argc>1)
{
sMissionFile = argv[1];
}
//this is a main MOOS DB Object
CMOOSDB DB;
DB.Run(sMissionFile);
//this is a webserver object which allows you
//to access and prod the MOOSDB via HTTP
#ifdef MOOSDB_HAS_WEBSERVER
long lWebServerPort = 0;
CMOOSFileReader missionReader;
if(missionReader.SetFile(sMissionFile))
{
std::string sWebServerPort;
if(missionReader.GetValue("WEBSERVERPORT",sWebServerPort))
{
lWebServerPort = atoi(sWebServerPort.c_str());
}
}
// Fire off the web server
std::auto_ptr<CMOOSDBHTTPServer> pWebServer;
if (lWebServerPort > 0)
pWebServer = std::auto_ptr<CMOOSDBHTTPServer> (new CMOOSDBHTTPServer(DB.GetDBPort(), lWebServerPort));
else
pWebServer = std::auto_ptr<CMOOSDBHTTPServer> (new CMOOSDBHTTPServer(DB.GetDBPort()));
#endif
//nothing to do - all the threads in the DB object
//do the work
while(1)
{
MOOSPause(1000);
}
return 0;
}
bool CXbowDriver::Initialise()
{
double dfTwist = 0.0;
if(!m_pMissionFileReader->GetConfigurationParam("TWIST", dfTwist))
{
if(m_pPort->IsVerbose())
{
MOOSTrace("XbowDriver::Initialise(): COULD NOT READ TWIST: %d\n", dfTwist);
}
m_dfVehicleYToINSX = 0.0;
}
else
{
m_dfVehicleYToINSX = dfTwist;
if(m_pPort->IsVerbose())
{
MOOSTrace("XbowDriver::Initialise(): INS is TWISTed: %f\n", m_dfVehicleYToINSX);
}
}
//need to know when to how big the returned bundle is
m_pPort->SetIsCompleteReplyCallBack(XbowPortReadCallBack);
//set to polled moade in angles...
m_pPort->Write("P",1);
MOOSPause(100);
//set to angle mode
m_pPort->Write("a",1);
char Spare[10];
m_pPort->ReadNWithTimeOut(Spare,1);
if(Spare[0]!='A')
{
MOOSTrace("Unexpected reply when setting angle mode (expecting 'A')\n");
return false;
}
return true;
}
int main()
{
MOOS::PeriodicEvent P;
//100Hz
P.SetPeriod(0.01);
//install callback
P.SetCallback(f,NULL);
//start
P.Start();
while(N<1000)
{
MOOSPause(100); //do nought
}
//stop
P.Stop();
}
int main(int argc, char * argv[]){
//understand the command line
MOOS::CommandLineParser P(argc,argv);
std::string db_host="localhost";
P.GetVariable("--moos_host",db_host);
int db_port=9000;
P.GetVariable("--moos_port",db_port);
std::string my_name ="ex50";
P.GetVariable("--moos_name",my_name);
//configure the comms
MOOS::MOOSAsyncCommClient Comms;
Comms.SetOnConnectCallBack(OnConnect,&Comms);
//here we add per message callbacks
//first parameter is the channel nick-name, then the function
//to call, then a parameter we want passed when callback is
//invoked
Comms.AddMessageCallback("callbackA","V1",funcA,NULL);
//add a default handler
Comms.AddMessageCallback("default","*",DefaultMail,NULL);
//start the comms running
Comms.Run(db_host,db_port,my_name);
//for ever loop sending data
std::vector<unsigned char> data(1000);
for(;;){
MOOSPause(10);
Comms.Notify("V1",data); //for funcA
Comms.Notify("V2","This is stuff"); //will be caught by default
}
return 0;
}
bool CMOOSDBHTTPServer::Listen()
{
//ignore broken pipes as is standard for network apps
#ifndef _WIN32
signal(SIGPIPE,SIG_IGN);
#endif
while(!m_pMOOSComms->IsConnected())
MOOSPause(100);
DoBanner();
m_pListenSocket = new XPCTcpSocket(m_lWebServerPort);
try
{
m_pListenSocket->vSetReuseAddr(1);
m_pListenSocket->vBindSocket();
}
catch(XPCException & e)
{
UNUSED_PARAMETER(e);
MOOSTrace("Error binding to HTTP listen socket - Is there another HTTPDBServer Running?\n");
MOOSTrace("This HTTP Server Is Quitting\n");
delete m_pListenSocket;
m_pListenSocket = NULL;
return false;
}
//OK looks good...
while(1)
{
try
{
char sClientName[200];
//wait here for folk demanding attention
m_pListenSocket->vListen();
//let them in the door
XPCTcpSocket * pNewSocket = m_pListenSocket->Accept(sClientName);
//make a new handler object
CHTTPConnection * pNewConnection = new CHTTPConnection(pNewSocket,m_pMOOSComms,m_pMOOSCommsLock);
//let it run and service the client as it sees fit
//this is a non blocking call allowing the server to
//dal with other requests
pNewConnection->Run();
//store this connection
m_Connections.push_back(pNewConnection);
//look to clean up here..
std::list<CHTTPConnection*>::iterator q = m_Connections.begin();
while( q!=m_Connections.end())
{
//carefully remove expired handler objects
if((*q)->HasCompleted())
{
CHTTPConnection* t = *q;
q = m_Connections.erase(q);
delete t;
}
else
{
++q;
}
}
}
catch(XPCException & e)
{
MOOSTrace("Exception Thrown in listen loop: %s\n",e.sGetException());
}
}
delete m_pListenSocket;
return true;
}
bool CMOOSNavTopDownCalEngine::GetXYZ(double &dfX,
double &dfY,
double &dfZ,
double dfTime,
double dfTolerance)
{
dfZ = 0;
OBSLIST * pGPSX = m_pStore->GetListByType(CMOOSObservation::X);
OBSLIST * pGPSY = m_pStore->GetListByType(CMOOSObservation::Y);
if(pGPSX==NULL || pGPSY==NULL)
{
AddToOutput("TDC: Cannot locate GPS data..is iGPS running?");
MOOSPause(2000);
return false;
}
if(pGPSX->empty() || pGPSY->empty())
return false;
OBSLIST::iterator q,w;
w = pGPSX->end();
double dfMinDT = 1e9;
for(q = pGPSX->begin(); q!=pGPSX->end(); q++)
{
double dfDT =fabs((*q).m_dfTime -dfTime);
if( dfDT<dfTolerance && dfDT<dfMinDT)
{
w = q;
dfMinDT = dfDT;
}
}
if(w == pGPSX->end())
{
return false;
}
dfX = (*w).m_dfData;
dfMinDT = 1e9;
w = pGPSY->end();
for(q = pGPSY->begin(); q!=pGPSY->end(); q++)
{
double dfDT =fabs((*q).m_dfTime -dfTime);
if( dfDT<dfTolerance && dfDT<dfMinDT)
{
w = q;
dfMinDT = dfDT;
}
}
if(w == pGPSY->end())
{
return false;
}
dfY = (*w).m_dfData;
return true;
}
