Beispiel #1
0
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");
	}



}
Beispiel #2
0
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;
}
Beispiel #3
0
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;
}
Beispiel #5
0
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;
	}




}
Beispiel #7
0
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;
    
}
Beispiel #8
0
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;
        
}
Beispiel #9
0
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;
}
Beispiel #10
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;
}
Beispiel #11
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;
}
Beispiel #12
0
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();
}
Beispiel #13
0
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;
}
Beispiel #14
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;
}