int main(int argc, char *argv[]) {
// we need to set stdout and stderr non-buffered
// so that messages are actually displayed in the
// DARwIn-OP console of the robot window
setvbuf(stdout,NULL,_IONBF,0);
setvbuf(stderr,NULL,_IONBF,0);
int cameraWidthZoomFactor = 1;
int cameraHeightZoomFactor = 1;
if (argc >= 3) {
sscanf(argv[1], "%d", &cameraWidthZoomFactor);
sscanf(argv[2], "%d", &cameraHeightZoomFactor);
}
// Server socket
SOCKADDR_IN sin;
SOCKET sock;
socklen_t recsize = sizeof(sin);
// Client socket
SOCKADDR_IN csin;
SOCKET csock = NULL;
socklen_t crecsize;
int sock_err;
// Creation of the socket
sock = socket(AF_INET, SOCK_STREAM, 0);
// If socket is valid
if (sock != INVALID_SOCKET) {
// Configuration
sin.sin_addr.s_addr = htonl(INADDR_ANY); // Adresse IP automatic
sin.sin_family = AF_INET; // Protocole familial (IP)
sin.sin_port = htons(PORT); // Listening of the port
bzero(&sin.sin_zero, sizeof(sin.sin_zero)); // make sure the zero are correctly initialized
int opt = 1;
setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (const char *) &opt, sizeof(int));
sock_err = bind(sock, (SOCKADDR*) &sin, recsize);
// If socket works
if (sock_err != SOCKET_ERROR) {
// Starting port Listening (server mode)
sock_err = listen(sock, 1); // only one connexion
// If socket works
if (sock_err != SOCKET_ERROR) {
// Wait until a client connect
cout << "Waiting for client connection on port " << PORT << "..." << endl;
csock = accept(sock, (SOCKADDR*)&csin, &crecsize);
cout << "Client connected." << endl;
} else
perror("listen");
}
else
perror("bind");
Remote *remote = new Remote();
remote->remoteStep();
const double *acc;
const double *gyro;
const unsigned char *image;
char *receiveBuffer = (char *)malloc(1024);
char *sendBuffer = (char *)malloc(350000);
receiveBuffer[0]='\0';
sendBuffer[0]='\0';
Image rgbImage((320/cameraWidthZoomFactor), (240/cameraHeightZoomFactor), Image::RGB_PIXEL_SIZE);
unsigned char jpeg_buffer[rgbImage.m_ImageSize];
int c = 0;
int n;
while(1) {
// Wait for message
n=0;
do
n += recv(csock, &receiveBuffer[n], 1024-n, 0);
while(n<3);
if (receiveBuffer[0]!='W') {
cerr << "Error: wrong TCP message received" << endl;
continue;
}
int total = (unsigned char)receiveBuffer[1]+(unsigned char)receiveBuffer[2]*256;
while(n<total)
n += recv(csock, &receiveBuffer[n], 1024-n, 0);
receiveBuffer[total]=0; // set the final 0
int receivePos = 3, sendPos = 5;
// Accelerometer
if (receiveBuffer[receivePos] == 'A') {
acc = remote->getRemoteAccelerometer();
for (c = 0; c < 3; c++)
writeINT2Buffer(sendBuffer + 4 * c + sendPos, (int)acc[c]);
sendPos += 12;
receivePos++;
}
// Gyro
if (receiveBuffer[receivePos] == 'G') {
gyro = remote->getRemoteGyro();
for (c = 0; c < 3; c++)
writeINT2Buffer(sendBuffer + 4 * c + sendPos, (int)gyro[c]);
sendPos += 12;
receivePos++;
}
// Camera
if (receiveBuffer[receivePos] == 'C') {
image = remote->getRemoteImage();
int image_buffer_position = 0;
for (int height = 120 - (120 / cameraHeightZoomFactor) ; height < 120 + (120 / cameraHeightZoomFactor); height++) {
for (int width = 160 - (160 / cameraWidthZoomFactor) ; width < 160 + (160 / cameraWidthZoomFactor); width++) {
rgbImage.m_ImageData[image_buffer_position + 2] = image[height * 320 * 4 + width * 4 + 0];
rgbImage.m_ImageData[image_buffer_position + 1] = image[height * 320 * 4 + width * 4 + 1];
rgbImage.m_ImageData[image_buffer_position + 0] = image[height * 320 * 4 + width * 4 + 2];
image_buffer_position += 3;
}
}
// Compress image to jpeg
int buffer_length = 0;
if (cameraHeightZoomFactor *cameraWidthZoomFactor < 2) // -> resolution 320x240 -> put quality at 65%
buffer_length = jpeg_utils::compress_rgb_to_jpeg(&rgbImage, jpeg_buffer, rgbImage.m_ImageSize, 65);
else // image smaller, put quality at 80%
buffer_length = jpeg_utils::compress_rgb_to_jpeg(&rgbImage, jpeg_buffer, rgbImage.m_ImageSize, 80);
writeINT2Buffer(sendBuffer + sendPos, buffer_length); // write image_buffer length
sendPos += 4;
memcpy(sendBuffer + sendPos, jpeg_buffer, buffer_length); // write image
sendPos += buffer_length;
receivePos++;
}
// LEDs
for (c = 0; c < 5; c++) {
if (receiveBuffer[receivePos] == 'L') {
unsigned char c1 = static_cast <unsigned char> (receiveBuffer[receivePos+4]);
unsigned char c2 = static_cast <unsigned char> (receiveBuffer[receivePos+3]);
unsigned char c3 = static_cast <unsigned char> (receiveBuffer[receivePos+2]);
int value = c1 + (c2 << 8) + (c3 << 16);
remote->setRemoteLED(receiveBuffer[receivePos+1], value);
receivePos += 5;
}
}
// Motors Actuator
for (c = 0; c < 20; c++) {
if (receiveBuffer[receivePos] == 'S') {
int motorNumber = (int)receiveBuffer[receivePos+1];
receivePos+= 2;
if (receiveBuffer[receivePos] == 'p') { // Position
int value = readINTFromBuffer(receiveBuffer + receivePos + 1);
remote->setRemoteMotorPosition(motorNumber, value);
receivePos += 5;
}
if (receiveBuffer[receivePos] == 'v') { // Velocity
int value = readINTFromBuffer(receiveBuffer + receivePos + 1);
remote->setRemoteMotorVelocity(motorNumber, value);
receivePos += 5;
}
if (receiveBuffer[receivePos] == 'a') { // Acceleration
int value = readINTFromBuffer(receiveBuffer + receivePos + 1);
remote->setRemoteMotorAcceleration(motorNumber, value);
receivePos += 5;
}
if (receiveBuffer[receivePos] == 'm') { // AvailableTorque
int value = readINTFromBuffer(receiveBuffer + receivePos + 1);
remote->setRemoteMotorAvailableTorque(motorNumber, value);
receivePos += 5;
}
if (receiveBuffer[receivePos] == 'c') { // ControlPID
int p = readINTFromBuffer(receiveBuffer + receivePos + 1);
int i = readINTFromBuffer(receiveBuffer + receivePos + 1 + 4);
int d = readINTFromBuffer(receiveBuffer + receivePos + 1 + 2 * 4);
remote->setRemoteMotorControlPID(motorNumber, p, i, d);
receivePos += 1 + 3 * 4; // TODO (fabien): why not use sizeof(int) ?
}
if (receiveBuffer[receivePos] == 'f') { // Torque
int value = readINTFromBuffer(receiveBuffer + receivePos + 1);
remote->setRemoteMotorTorque(motorNumber, value);
receivePos += 5;
}
}
}
// Position Sensors
for (c = 0; c < 20; c++) {
if (receiveBuffer[receivePos] == 'P') {
if ((int)receiveBuffer[receivePos+1] < 20) {
double motorPosition = remote->getRemotePositionSensor((int)receiveBuffer[receivePos+1]);
writeINT2Buffer(sendBuffer + sendPos, (int)motorPosition);
}
else
writeINT2Buffer(sendBuffer + sendPos, 0);
sendPos += 4;
receivePos += 2;
}
}
// Motors sensors torque
for (c = 0; c < 20; c++) {
if (receiveBuffer[receivePos] == 'F') {
if ((int)receiveBuffer[receivePos+1] < 20) {
double motorTorque = remote->getRemoteMotorTorque((int)receiveBuffer[receivePos+1]);
writeINT2Buffer(sendBuffer + sendPos, (int)motorTorque);
} else
writeINT2Buffer(sendBuffer + sendPos, 0);
sendPos += 4;
receivePos += 2;
}
}
if (receiveBuffer[receivePos]!=0)
cerr << "Error: received unknown message: " << receiveBuffer[receivePos] << endl;
// Terminate the buffer and send it
sendBuffer[0] = 'W';
writeINT2Buffer(sendBuffer+1, sendPos); // Write size of buffer at the beginning
sendBuffer[sendPos++] = '\0';
n=0;
do
n += send(csock, &sendBuffer[n], sendPos-n, 0);
while (n!=sendPos);
remote->remoteStep();
}
// Close client socket and server socket
cout << "Closing client socket" << endl;
closesocket(csock);
cout << "Closing server socket" << endl;
closesocket(sock);
free(receiveBuffer);
free(sendBuffer);
}
else
perror("socket");
return EXIT_SUCCESS;
}
