/*
* Server initialization routine
*/
void
init(void)
{
printf("Initializing... ");
fflush(stdout);
ipc_packet_data.ipc_method = SHMEM;
ipc_packet_data.server = true;
ipc_packet_data.reqresp = NULL;
ipc_packet_data.shmem = NULL;
setupSemaphores();
if(initIPC(&ipc_packet_data) < 0)
{
ipcerror("mtimesrv::init():initIPC() - failed");
exit(1);
}
status.just_started = true;
status.ref_count = 0;
if(ipc_packet_data.ipc_method == SHMEM)
status.shm_id = ipc_packet_data.resource_id;
printf("Done.\n");
fflush(stdout);
}
retorno *
listadovendedores_2_svc(void *argp, struct svc_req *rqstp)
{
static retorno result;
initIPC();
NumeradorInfo* numeradorInfo;
wait(0);
{
numeradorInfo = (NumeradorInfo*)data;
strcpy (result.retorno_u.informacion, numeradorInfo->numerosVendedores); /* asignacion de string segun c */
}
signal(0);
result.cod_ret = 1;
return &result;
}
retorno *
listadoclientes_1_svc(void *argp, struct svc_req *rqstp)
{
static retorno result;
ClientesInfo* numeradorInfo;
initIPC();
wait(0);
{
numeradorInfo = (ClientesInfo*)data;
memcpy(result.retorno_u.informacion,numeradorInfo->idClientes,MAX_CLIENTES);
}
signal(0);
result.cod_ret = 1;
return &result;
}
retorno *
obteneridcliente_1_svc(void *argp, struct svc_req *rqstp)
{
static retorno result;
ClientesInfo* numeradorInfo;
int numero = 0;
char exito = 0;
int i = 0;
initIPC();
printf("Intentando obtener un cliente libre.\n");
wait(0);
{
numeradorInfo = (ClientesInfo*)data;
while ((exito == 0) && (i < MAX_CLIENTES)) {
printf("Verificando cliente numero: %d: %d.\n",i+1, numeradorInfo->idClientes[i]);
if (numeradorInfo->idClientes[i] == 0) {
printf("Cliente libre: %d.\n",i+1);
numero = i+1;
exito = 1;
numeradorInfo->idClientes[i] = 1;
}
++i;
}
if (exito == 1) {
writeInfo(numeradorInfo);
}
}
signal(0);
result.retorno_u.numero = numero;
if (exito == 1) {
result.cod_ret = 1;
printf("Cliente libre obtenido: %d.\n", numero);
}
else {
result.cod_ret = 2;
printf("No hay clientes libres disponibles.\n");
}
return &result;
}
retorno *
numeradorvendedor_2_svc(void *argp, struct svc_req *rqstp)
{
printf("Intentando obtener un vendedor libre.\n");
static retorno result;
initIPC();
NumeradorInfo* numeradorInfo;
int numero = 0;
char exito = 0;
wait(0);
{
numeradorInfo = (NumeradorInfo*)data;
int i = 0;
while ((exito == 0) && (i < MAX_VENDEDORES)) {
if (numeradorInfo->numerosVendedores[i] == 0) {
numero = i+1;
exito = 1;
numeradorInfo->numerosVendedores[i] = 1;
}
++i;
}
if (exito == 1) {
writeInfo(numeradorInfo);
}
}
signal(0);
result.retorno_u.numero = numero;
if (exito == 1) {
result.cod_ret = 1;
printf("Vendedor libre obtenido: %d.", numero);
}
else {
result.cod_ret = 2;
printf("No hay vendedores libres disponibles.");
}
result.retorno_u.numero = 8;
result.cod_ret = 1;
return &result;
}
void *
devolveridcliente_1_svc(int *argp, struct svc_req *rqstp)
{
static char * result;
ClientesInfo* numeradorInfo;
printf("Devolviendo el cliente: %d.\n", (*argp));
initIPC();
wait(0);
{
numeradorInfo = (ClientesInfo*)data;
numeradorInfo->idClientes[(*argp)-1] = 0;
writeInfo(numeradorInfo);
}
signal(0);
return (void *) &result;
}
void *
devolvervendedor_2_svc(int *argp, struct svc_req *rqstp)
{
printf("Devolviendo el vendedor: %d.\n", (*argp));
static char * result;
initIPC();
NumeradorInfo* numeradorInfo;
wait(0);
{
numeradorInfo = (NumeradorInfo*)data;
numeradorInfo->numerosVendedores[(*argp)-1] = 0;
writeInfo(numeradorInfo);
}
signal(0);
return (void *) &result;
}
/*
* Init IPC, connect to the snoop server
*/
void
connectSnoopSrv(char* host)
{
/* Init client's IPC */
printf("Intializing IPC...\n");
printf("Connecting to host [%s]\n", host);
ipc_packet_data.ipc_method = SOCKETS;
ipc_packet_data.shmem = NULL;
ipc_packet_data.extra = NULL;
ipc_packet_data.reqresp = host;
ipc_packet_data.server = false;
if(initIPC(&ipc_packet_data) < 0)
{
ipcerror("microsh::connectSnoopSrv():initIPC() - failed");
return; /* this is not fatal, client still may operate as a shell */
}
status.connected = true;
status.curr_signal = SH_REMOTE;
printf("Done.\n");
/*srvVersion();*/
/* Open a session */
{
request_t register_cli;
requestPrepare(®ister_cli);
register_cli.command = status.curr_cmd == SRV_RSHELL ? RSHELL : RSNOOP;
srvSendReceive();
}
}
void *
resetearclientes_1_svc(void *argp, struct svc_req *rqstp)
{
static char * result;
ClientesInfo* numeradorInfo;
int i;
printf("Reseteando clientes...\n");
initIPC();
wait(0);
{
numeradorInfo = (ClientesInfo*)data;
for (i = 0; i < MAX_CLIENTES; ++i) {
numeradorInfo->idClientes[i] = 0;
}
writeInfo(numeradorInfo);
}
signal(0);
return (void *) &result;
}
/*
* Init IPC, connect to the server and register within it.
*/
void
connectTimeSrv(void)
{
/* Init client's IPC */
request_t register_cli;
printf("Intializing IPC... ");
setupSemaphores();
ipc_packet_data.ipc_method = SHMEM;
ipc_packet_data.shmem = NULL;
ipc_packet_data.extra = NULL;
ipc_packet_data.reqresp = NULL;
ipc_packet_data.server = false;
if(initIPC(&ipc_packet_data) < 0)
{
ipcerror("microsh::connectTimeSrv():initIPC() - failed");
return; /* this is not fatal, client still may operate as a shell */
}
/* Send register request */
requestPrepare(®ister_cli);
register_cli.command = OPEN;
if(sendRequest(&ipc_packet_data) < 0)
{
ipcerror("microsh::connectTimeSrv():sendRequest() - failed");
return; /* this is not fatal, client still may operate */
}
status.connected = true;
printf("Done.\n");
}
void *
resetearvendedores_2_svc(void *argp, struct svc_req *rqstp)
{
static char * result;
printf("Reseteando vendedores...\n");
initIPC();
NumeradorInfo* numeradorInfo;
wait(0);
{
numeradorInfo = (NumeradorInfo*)data;
int i;
for (i = 0; i < MAX_CLIENTES; ++i) {
numeradorInfo->numerosVendedores[i] = 0;
}
writeInfo(numeradorInfo);
}
signal(0);
return (void *) &result;
}
int main(int argc, char *argv[]){
GCM_Common_RobotState curState;
bool quit=false;
char *pointer;
GCM_Common_Command *gcc = (GCM_Common_Command *)malloc(sizeof(GCM_Common_Command));
GCM_logSetFilename((char *)"snmd-control.log");
initIPC((char *)"SNMD::Control", argv[1], &curState);
printf("SNMD command interface. Press '?' for more information.\n");
strncpy(gcc->name,"nav",64);
while(!quit){
IPC_listenClear(100);
pointer=readline("> ");
switch(pointer[0]){
case '?':
printf("Command functions:\n");
printf("==================\n");
printf("?: This screen\n");
printf("a: Issue an achieve command with (x,y) coordinates\n");
printf("b: Wall following backward\n");
printf("c: Check SNMD status\n");
printf("f: Wall following forward\n");
printf("g: Issue an achieve command with (x,y,theta) coordinates \n");
printf("h: Halt the robot (idle)\n");
printf("m: Issue a move command\n");
printf("o: Issue an orient command\n");
printf("r: Wander\n");
printf("s: Soft stop\n");
printf("t: Run the testbed\n");
printf("w: Use waypoint following\n");
printf("x: Exit snmd-control but don't halt robot\n");
printf("q: Quit snmd-control and halt SNMD\n");
break;
case 'c':
printf("Position: %.2f %.2f %.2f\n", curState.x, curState.y, curState.t );
break;
case 'p':
// tell the robot to achieve
pointer = readline("% X: ");
gcc->data[0] = atof(pointer);
pointer = readline("% Y: ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // punt on the angle here
printf("Sending achieve point %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "absolute");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'a':
// tell the robot to achieve a relative location
pointer = readline("% dX(m): ");
gcc->data[0] = atof(pointer);
pointer = readline("% dY(m): ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // punt on the angle here
printf("Sending relative achieve %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "relative");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'd':
// tell the robot to approach (forward or backward)
pointer = readline("% dX(m): ");
gcc->data[0] = atof(pointer);
pointer = readline("% dApp(m): ");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0; // no need for an angle here
printf("Sending approach %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "approach");
gcc->mode = GCM_NAV_ACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'g':
// tell the robot to go to a relative location
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "sn relative");
gcc->mode = GCM_NAV_ACHIEVE;
pointer = readline("% dX(m): ");
// enter "verbose" instead of an x value to short-circuit the command
// and instead toggle verbose mode for the achieve_sn competency
if (!strncmp(pointer, "verbose", 16)) {
strcpy(gcc->cdata, "toggle verbose");
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
}
gcc->data[0] = atof(pointer);
pointer = readline("% dY(m): ");
gcc->data[1] = atof(pointer);
pointer = readline("% dT(m): ");
gcc->data[2] = atof(pointer);
printf("Sending relative achieve %.2f %.2f %.2f\n",
gcc->data[0], gcc->data[1], gcc->data[2]);
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'm':
// tell the robot to move
pointer=readline("% Trans Vel (m/s)? ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
pointer=readline("% Rot Vel (rad/s)? ");
gcc->data[1]=atof(pointer);
printf("Data: %f\n",gcc->data[1]);
gcc->mode=GCM_NAV_MOVE;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
case 't':
gcc->mode = GCM_PLAN_PATH;
strncpy(gcc->name, "planner", 64);
pointer = readline("% X (meters): ");
gcc->data[0] = atof(pointer);
pointer = readline("% Y (meters): ");
gcc->data[1] = atof(pointer);
IPC_publishData(GCM_COMMON_COMMAND, gcc);
strncpy(gcc->name, "nav", 64);
break;
/*
printf("snmd-control.c: Entering the testbed case.\n");
// tell the robot to move
pointer=readline("% Max Trans? ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
pointer=readline("% Max Rotation? ");
gcc->data[1]=atof(pointer);
printf("Data: %f\n",gcc->data[1]);
gcc->mode=GCM_NAV_TEST;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
*/
case 'o':
// tell the robot to orient
pointer=readline("Angle (rad) ");
gcc->data[0]=atof(pointer);
printf("Data: %f\n",gcc->data[0]);
gcc->mode=GCM_NAV_ANGACHIEVE;
IPC_publishData(GCM_COMMON_COMMAND,gcc);
break;
case 'w':
{
int i;
pointer = readline(" % Waypoints:");
int numWaypoints = atoi(pointer);
printf("Calculation method is (a)rcs and circles, (s)plines, sta(n)dard?:\n");
pointer = readline(">> ");
strncpy( gcc->cdata, pointer, 64 );
for (i=0; i < numWaypoints; i++)
{
pointer = readline ("% X:");
gcc->data[0] = atof(pointer);
pointer = readline("% Y:");
gcc->data[1] = atof(pointer);
gcc->data[2] = 0.0;
gcc->data[3] = i; // stores the current waypoint number
printf("Adding point to the queue %.2f %.2f\n", gcc->data[0], gcc->data[1]);
strcpy(gcc->name, "nav");
gcc->mode = GCM_NAV_WAYPOINT;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
}
gcc->mode = GCM_NAV_EXECWP;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
}
case 'r':
// tell nav to wander
printf("Sending wander...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wander");
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 's': // soft stop
// stop planner module if it was running
strcpy(gcc->name, "planner");
gcc->mode = GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
// coast robot to stop
printf("Soft stop...\n");
strcpy(gcc->name, "nav");
strcpy(gcc->cdata, "soft stop");
gcc->mode = GCM_NAV_AUTO;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
break;
case 'h': // hard stop (halt)
// stop planner module if it was running
strcpy(gcc->name, "planner");
gcc->mode = GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc);
// tell nav to idle
printf("Halt! Sending idle...\n");
strcpy(gcc->name, "nav");
gcc->mode=GCM_COMMON_IDLE;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 'x':
quit = true;
break;
case 'q':
printf("G'bye.\n");
gcc->mode=GCM_COMMON_QUIT;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
quit=true;
break;
case 'f':
// tell nav to wall follow forward
printf("Sending wall follow...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wallFollow");
gcc->data[0] = WALLFOLLOW_FORWARD;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
case 'b':
// tell nav to wall follow backward
printf("Sending wall follow...\n");
gcc->mode=GCM_NAV_AUTO;
strcpy(gcc->cdata, "wallFollow");
gcc->data[0] = WALLFOLLOW_BACKWARD;
IPC_publishData(GCM_COMMON_COMMAND, gcc );
break;
default:
printf("Unknown command %c\n", pointer[0]);
break;
}
free(pointer);
}
IPC_disconnect();
printf("Goodbye...\n");
return 0;
}
