static int createBinarySemaphore(key_t sem_key, int shm_id)
{
union semun sem_conf;
int sem_id = semget(sem_key, NUM_OF_SEMAPHORES, IPC_CREAT|ALLOW_READ_WRITE_TO_ALL);
if(sem_id < 0)
{
deleteFifo();
deleteSharedMemory(shm_id);
// No checking needed, exits with error code.
write(STDERR_FILENO, SEMGET_ERROR, sizeof(SEMGET_ERROR));
exit(EXIT_ERROR_CODE);
}
sem_conf.val = SEM_INIT;
if(semctl(sem_id, 0, SETVAL, sem_conf) < 0)
{
deleteFifo();
deleteResources(shm_id, sem_id);
// No checking needed, exits with error code.
write(STDERR_FILENO, SEMCTL_ERROR, sizeof(SEMCTL_ERROR));
exit(EXIT_ERROR_CODE);
}
return sem_id;
}
int main(int argc, char **argv)
{
createSharedMemory();
ipcon_create(&MyIP);
// Connect to brickd
if(ipcon_connect(&MyIP, HOST, PORT) < 0) {
fprintf(stderr, "Could not connect to brickd\n");
exit(1);
}
getTinker();
sleep(2); // Es dauert etwas, bis die Enumeration der Devices durch ist, also warten !!!
if ( tinkerCounter <= 0 ) {
fprintf(stderr,"Kein Gerät vorhanden ");
exit(2);
}
curses_init();
showSystem(1);
// Threads für die Anzeige erstellen
tf1_init();
getchar();
curses_end();
deleteSharedMemory();
return (0);
}
char *createSharedMemory(key_t key, int *shm_id)
{
char *p_shm_data = NULL;
// Create shared memory
*shm_id = shmget(key, SHM_SIZE_IN_BYTES, IPC_CREAT|ALLOW_READ_WRITE_TO_ALL);
if(*shm_id < 0)
{
deleteFifo();
// No checking needed, exits with error code.
write(STDERR_FILENO, SHMGET_ERROR, sizeof(SHMGET_ERROR));
exit(EXIT_ERROR_CODE);
}
// Connect to the shared memory.
p_shm_data = (char *)shmat(*shm_id, 0, 0);
if(SHMAT_FAILED == p_shm_data)
{
deleteFifo();
deleteSharedMemory(*shm_id);
// No checking needed, exits with error code.
write(STDERR_FILENO, SHMAT_ERROR, sizeof(SHMAT_ERROR));
exit(EXIT_ERROR_CODE);
}
return p_shm_data;
}
static void deleteResources(int shm_id, int sem_id)
{
union semun ignored_arg;
deleteSharedMemory(shm_id);
if(semctl(sem_id, NUM_OF_SEMAPHORES, IPC_RMID, ignored_arg) < 0)
{
// No checking needed, exits with error code.
write(STDERR_FILENO, SEMCTL_ERROR, sizeof(SEMCTL_ERROR));
exit(EXIT_ERROR_CODE);
}
printf("semaphore deleted.\n");
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
static bool firstTime = true;
initSemaphore(&stepSEM);
attachSEM(&stepSEM, STEP_SEM);
initSharedMemory(&stepSHM, sizeof(episode_steps));
attachSHM(&stepSHM, STEP_SHM, &stepSEM);
readSHMemory(&stepSHM, &episodeStep, &stepSEM);
int numStates = (int) *mxGetPr(prhs[0]);
int numSteps = episodeStep.numTransmittedSteps;
fprintf(stderr, "NumSteps: %d\n",numSteps);
plhs[0] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[1] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[2] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[3] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[4] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[5] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[6] = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
plhs[7] = mxCreateDoubleMatrix(7, numSteps, mxREAL);
plhs[8] = mxCreateDoubleMatrix(numStates, numSteps, mxREAL);
plhs[9] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
plhs[10] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
plhs[10] = mxCreateDoubleMatrix(1, numSteps, mxREAL);
double *joints = mxGetPr(plhs[0]);
double *jointsVel = mxGetPr(plhs[1]);
double *jointsAcc = mxGetPr(plhs[2]);
double *jointsDes = mxGetPr(plhs[3]);
double *jointsVelDes = mxGetPr(plhs[4]);
double *jointsAccDes = mxGetPr(plhs[5]);
double *torque = mxGetPr(plhs[6]);
double *cart = mxGetPr(plhs[7]);
double *state = mxGetPr(plhs[8]);
double *commandIdx = mxGetPr(plhs[9]);
double *stepInTrajectory = mxGetPr(plhs[10]);
double *doMotionIdx = mxGetPr(plhs[10]);
memcpy(joints, episodeStep.joints, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsVel, episodeStep.jointsVel, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsAcc, episodeStep.jointsAcc, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsDes, episodeStep.jointsDes, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsVelDes, episodeStep.jointsVelDes, sizeof(double) * numSteps * N_DOFS);
memcpy(jointsAccDes, episodeStep.jointsAccDes, sizeof(double) * numSteps * N_DOFS);
memcpy(torque, episodeStep.torque, sizeof(double) * numSteps * N_DOFS);
memcpy(cart, episodeStep.cart, sizeof(double) * numSteps * 7);
// printf("doMotionIdx :");
// for (int i = 0; i < numSteps; i ++)
// printf(" %d",episodeStep.doMotionIdx[i]);
// printf(" \n");
for (int i = 0; i < numSteps; i ++)
{
for (int j = 0; j < numStates; j ++)
{
state[i * numStates + j] = episodeStep.state[i][j];
// printf("%f ", episodeStep.state[i][j]);
}
// printf("\n");
}
for (int i = 0; i < numSteps; i ++)
{
commandIdx[i] = episodeStep.commandIdx[i];
doMotionIdx[i] = episodeStep.doMotionIdx[i];
}
for (int i = 0; i < numSteps; i ++)
{
stepInTrajectory[i] = episodeStep.stepInTrajectory[i];
}
deleteSemaphore(&stepSEM);
deleteSharedMemory(&stepSHM);
}
