help needed!! I have the following program in C language, that takes initial and final pose of a robot and moves it to the final state. I want to simulate the process for a turtlebot in ROS/GAZEBO, so i need to "translate" the program to a client node!!! here is the code:

include <stdio.h>

include <math.h>

include <stdlib.h>

void control(double Xinit[3], double Xfinal[3], double u1, double w1) //Η συνάρτηση επιστρέφει τις ταχύτητες u και ω

{ FILE *file; file = fopen("file.txt","a+");

double f, th, a, x, y, e, p, csa, sna, u2, w2, Xk[3];

double pi=3.141592654;

int g = 3; int h = 1; int k = 6;

int i;

x = Xfinal[0] - Xinit[0]; //Το πρώτο στοιχείο του διανύσματος Χfinal αντιστοιχεί στη τελική συντεταγμένη - Χ, ενώ το πρώτο στοιχείο του διανύσματος Χinit αντιστοιχεί στη συντεταγμένη - Χ του ρομπότ κάθε χρονικη στιγμή κατά τη διάρκεια της πορείας του.

y = Xfinal[1] - Xinit[1]; //Το δεύτερο στοιχείο του διανύσματος Χfinal αντιστοιχεί στη τελική συντεταγμένη - Υ, ενώ το δεύτερο στοιχείο του διανύσματος Χinit αντιστοιχεί στη συντεταγμένη - Υ του ρομπότ κάθε χρονικη στιγμή κατά τη διάρκεια της πορείας του.

f = Xfinal[2] - Xinit[2]; //Το τρίτο στοιχείο του διανύσματος Χfinal αντιστοιχεί στο τελικό προσανατολισμό, ενώ το τρίτο στοιχείο του διανύσματος Χinit αντιστοιχεί στο προσανατολισμό του ρομπότ κάθε χρονικη στιγμή κατά τη διάρκεια της πορείας του.

p = atan2(y,x); p = p*180/pi;

th = Xfinal[2] - p;

a = th - f;

if(a>0) a=-a;

csa = cos(api/180); sna = sin(api/180);

e = sqrt(xx + yy);

//Υπολογισμός γραμμικής ταχύτητας - u u2 = u1 + (egcsa);

//Υπολογισμός γωνιακής ταχύτητας - ω w2 = w1 + (ka + ((gcsasna(a + h*th))/a));

//fprintf(file,"\nDx = %f",x); //fprintf(file,"\nDy = %f",y); //fprintf(file,"\nDf = %f",f);

//fprintf(file,"\nth = %f",th); //fprintf(file,"\na = %f",a); //fprintf(file,"\ne = %f",e);

fprintf(file,"\n%10.3f",u2); fprintf(file,"%10.3f",w2);

fclose(file);

for(i=0; i<3; i++) Xk[i]=0.0;

model(Xk,Xinit,u2,w2,Xfinal);

return; }

void model(double Xk[3], double Xj[3], double u1, double w1, double Xfinal[3]) //Οι ταχύτητες u και ω, υπολογίζονται στη συνάρτηση control

{ FILE *file; file = fopen("file.txt","a");

double B[3][2];

double Dt;

double pi=3.141592654;

Dt=0.0005;

B[0][0] = cos(Xj[2]pi/180); B[0][1] = 0.0; B[1][0] = sin(Xj[2]pi/180); B[1][1] = 0.0; B[2][0] = 0.0; B[2][1] = 1.0;

if(Xj[0] >= Xfinal[0]) Xk[0] = Xj[0] + (B[0][0]u1 + B[0][1]w1)*Dt; else Xk[0] = Xj[0];

if(Xj[1] >= Xfinal[1]) Xk[1] = Xj[1] + (B[1][0]u1 + B[1][1]w1)*Dt; else Xk[1] = Xj[1];

if(Xj[2] >= Xfinal[2]) Xk[2] = Xj[2] + (B[2][0]u1 + B[2][1]w1)*Dt; else Xk[2] = Xj[2];

if ... (more)