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;
}
}
/* === 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 ----- */
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;
}
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;
}
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);
}
}
/* === 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();
*/
}
}