void getDeviceState (double start_proxyxform[], bool &start_proxybutton, double end_proxyxform[], bool &end_proxybutton) {
    cout << "receive?" << endl;
    if (receiver.recv(buf)) {
        while (receiver.recv(buf));
        
        cout << buf << endl;
        
        boost::split(vectbuf, buf, boost::is_any_of("\n"));
        parse(vectbuf[0], vect1);
        parse(vectbuf[1], vect2);
        /*
        int i;
        for (i=0;i<vect1.size();i++)
        cout << vect1[i] << endl;
        for (i=0;i<vect1.size();i++)
        cout << vect2[i] << endl;
        */
        // Extract transform matrix
        for (i=0; i<16; i++) {
            start_proxyxform[i] = boost::lexical_cast<double>(vect1[i].c_str());
            end_proxyxform[i]   = boost::lexical_cast<double>(vect2[i].c_str());
        }
        //cout << vect1[16] << "\t" << vect2[16] << endl;
        start_proxybutton = boost::lexical_cast<int>(vect1[16].c_str()); //? true : false;
        end_proxybutton   = boost::lexical_cast<int>(vect2[16].c_str()); //? true : false;
    }
}
Esempio n. 2
0
/* ===  FUNCTION  ==============================================================
 *         Name:  listenerFlow
 *  Description:  The pig listens for incoming messages here. This is the only
 *                way to trigger events on a pig. Everything on the pig is in
 *                response to some message.
 * =============================================================================
 */
static void listenerFlow (int listenerPort)
{

  gPigOwnNode.portNumber = listenerPort;
  UDPSocket listenSocket (COM_IP_ADDR, listenerPort);

  while (true)
  {
    // Block for msg receipt
    int inMsgSize;
    char *inMsg;
    inMsg = (char *)malloc (MAX_MSG_SIZE);
    memset(inMsg, 0, MAX_MSG_SIZE);
    try
    {
      inMsgSize = listenSocket.recv(inMsg, MAX_MSG_SIZE);
    }
    catch (SocketException &e)
    {
      cout<<gPigOwnNode.portNumber<<": "<<e.what()<<endl;
    }
    inMsg[inMsgSize] = '\0';

    thread handlerThread (pigMsgHandler, inMsgSize, inMsg);
    handlerThread.detach();
  }
}		/* -----  end of function listenerFlow  ----- */
Esempio n. 3
0
int UDPSocketTest::run_test()
{
	cout << "UDP Socket test" << endl;

	UDPSocket* sSocket = new UDPSocket(3346);
	UDPSocket* cSocket = new UDPSocket();
	string message = "Test 1234567890";
	printf("sending\n");
	cSocket->sendTo(message,"127.0.0.1", 3346);
	char buffer[100];
	memset((void*)buffer,0,100);
	int rc = sSocket->recv(buffer, 100);
	cout<<"send msg:"<<message<<endl;
	cout<<"recv msg:"<<buffer<<endl;
	if (rc != (int)message.length()){
		perror("FAIL1: receive different message length");
	}
	if (message != buffer){
		perror("FAIL2: receive different message");
	}
	message = "Test Reply 129012901290";
	sSocket->reply(message);
	memset((void*)buffer,0,100);
	rc = cSocket->recv(buffer, 100);
	cout<<"send msg:"<<message<<endl;
	cout<<"recv msg:"<<buffer<<endl;
	if (rc != (int)message.length()){
		perror("FAIL1: receive different message length");
	}
	if (message != buffer){
		perror("FAIL2: receive different message");
	}
	sSocket->close();
	cSocket->close();
	delete sSocket;
	delete cSocket;
	printf("Bye bye..\n");
	return 0;
}
Esempio n. 4
0
int main(int argc, char *argv[]) {
  if ((argc < 3) || (argc > 4)) {   // Test for correct number of arguments
    cerr << "Usage: " << argv[0] 
         << " <Server> <Echo String> [<Server Port>]\n";
    exit(1);
  }

  string servAddress = argv[1];             // First arg: server address
  char* echoString = argv[2];               // Second arg: string to echo
  int echoStringLen = strlen(echoString);   // Length of string to echo
  if (echoStringLen > ECHOMAX) {    // Check input length
    cerr << "Echo string too long" << endl;
    exit(1);
  }
  unsigned short echoServPort = Socket::resolveService(
    (argc == 4) ? argv[3] : "echo", "udp");

  try {
    UDPSocket sock;
  
    // Send the string to the server
    sock.sendTo(echoString, echoStringLen, servAddress, echoServPort);
  
    // Receive a response
    char echoBuffer[ECHOMAX + 1];       // Buffer for echoed string + \0
    int respStringLen;                  // Length of received response
    if ((respStringLen = sock.recv(echoBuffer, ECHOMAX)) != echoStringLen) {
      cerr << "Unable to receive" << endl;
      exit(1);
    }
  
    echoBuffer[respStringLen] = '\0';             // Terminate the string!
    cout << "Received: " << echoBuffer << endl;   // Print the echoed arg

    // Destructor closes the socket

  } catch (SocketException &e) {
    cerr << e.what() << endl;
    exit(1);
  }
  
  return 0;
}
Esempio n. 5
0
void UDPClient::service()
{
  char sendBuffer[ECHOMAX] = {};				// Buffer for send string (send)
  this->sendString.copy(sendBuffer, sendString.length(), 0);
  int echoStringLen;				    // Length of string to echo
  echoStringLen = strlen(sendBuffer);

  if (echoStringLen > ECHOMAX) {    // Check input length
    cerr << "Echo string too long" << endl;
    exit(1);
  }
  //unsigned short echoServPort = Socket::resolveService("echo", "udp");

  try {
    UDPSocket sock;

    // Send the string to the server
    sock.sendTo(sendBuffer, echoStringLen, servAddress, echoServPort);
  
    // Receive a response
    respStringLen = sock.recv(echoBuffer, ECHOMAX); // Length of received response

	echoBuffer[respStringLen] = '\0';             // Terminate the string!
    //cout << "Received: " << echoBuffer << endl;   // Print the echoed arg
  
	if(respStringLen > 0)
	{
		response = echoBuffer;
		parsing();
		//string temp; 
		//std::stringstream os(response);
		//os >> temp;
	}

    // Destructor closes the socket
	//sock.disconnect();
	//sock.cleanUp();

  } catch (SocketException &e) {
    cerr << e.what() << endl;
    exit(1);
  }
}
Esempio n. 6
0
/* ===  FUNCTION  ==============================================================
 *         Name:  listenerFlow
 *  Description:  The bird listens for incoming messages here
 * =============================================================================
 */
static void listenerFlow ()
{
 UDPSocket listenSocket (DB_LISTEN_PORT);

  while (true) {
    // Block for msg receipt
    int inMsgSize;
    char *inMsg;
    inMsg = new char[MAX_MSG_SIZE]();
    try {
      inMsgSize = listenSocket.recv(inMsg, MAX_MSG_SIZE);
    } catch (SocketException &e) {
      cout<<"DB: "<<e.what()<<endl;
    }
    inMsg[inMsgSize] = '\0';

    thread handlerThread (dbMsgHandler, inMsgSize, inMsg);
    handlerThread.detach();
  }
}   /* -----  end of function listenerFlow  ----- */
// (1) right         (2) left
void getDeviceState (Vector3d &leftPosition, Matrix3d &leftRotation, Vector3d &rightPosition, Matrix3d &rightRotation) {
    if (receiver.recv(buf)) {
        while (receiver.recv(buf));
    
        boost::split(vectbuf, buf, boost::is_any_of("\n"));
        parse(vectbuf[0], vect1);
        parse(vectbuf[1], vect2);
         
       /*
        // Build transformation matrix
        for (i=0; i<4; i++) {
            for (j=0; j<4; j++) {
                t1[i][j] = boost::lexical_cast<double>(vect1[i*4 + j].c_str());
                t2[i][j] = boost::lexical_cast<double>(vect2[i*4 + j].c_str());
            }
        }
        
        // Extract rotation matrix from transformation matrix
        t1.getRotationMatrix(r1);
        t2.getRotationMatrix(r2);
        for (i=0; i<3; i++) {
            for (j=0; j<3; j++) {
                leftRotation (i,j)  = r2[i][j];
                rightRotation (i,j) = r1[i][j];
            }
        }
        
        // Extract position vector from transformation matrix
        for (i=0; i<3; i++) {
            leftPosition(i)  = t2[3][i];
            rightPosition(i) = t1[3][i];
        }
        */

        // Extract rotation matrix from transformation matrix
        for (i=0; i<3; i++) {
            for (j=0; j<3; j++) {
                leftRotation(i,j) = boost::lexical_cast<double>(vect2[i*3 + j].c_str());
                rightRotation(i,j) = boost::lexical_cast<double>(vect1[i*3 + j].c_str());
            }
        }
        
        // Extract position vector from transformation matrix
        for (j=0; j<3; j++) {
            leftPosition(j)  = boost::lexical_cast<double>(vect2[9 + j].c_str());
            rightPosition(j) = boost::lexical_cast<double>(vect1[9 + j].c_str());
        }
             
        /*
        for (i=0; i<3; i++)
            leftPosition(i) = boost::lexical_cast<double>(vect2[i+9].c_str());
        for (i=0; i<3; i++)
            leftRotation(0,i) = boost::lexical_cast<double>(vect2[i].c_str());
        for (i=0; i<3; i++)
            leftRotation(1,i) = boost::lexical_cast<double>(vect2[i+3].c_str());
        for (i=0; i<3; i++)
            leftRotation(2,i) = boost::lexical_cast<double>(vect2[i+6].c_str());
        //leftRotation.transpose();
            
        for (i=0; i<3; i++)
            rightPosition(i) = boost::lexical_cast<double>(vect1[i+9].c_str());
        for (i=0; i<3; i++)
            rightRotation(0,i) = boost::lexical_cast<double>(vect1[i].c_str());
        for (i=0; i<3; i++)
            rightRotation(1,i) = boost::lexical_cast<double>(vect1[i+3].c_str());
        for (i=0; i<3; i++)
            rightRotation(2,i) = boost::lexical_cast<double>(vect1[i+6].c_str());
        //rightRotation.transpose();
        */
    }
}