INT32 iPmdDMNChildProc::active()
{
INT32 rc = SDB_OK;
rc = attachSHM();
if ( SDB_OK == rc )
{
UINT32 i = 3;
UINT32 sn = _procInfo->sn;
while( i-- > 0 )
{
if ( sn != _procInfo->sn &&
(sn + 1) != _procInfo->sn )
{
rc = SDB_PERM;
PD_RC_CHECK( rc, PDERROR, "Don't repeat start the process" );
}
ossSleep( 2 * OSS_ONE_SEC ) ;
}
_procInfo->szTag[ 0 ] = 0 ;
_procInfo->init() ;
}
else
{
rc = allocSHM();
PD_RC_CHECK( rc, PDERROR, "Failed to allocate share-memory(rc=%d)",
rc ) ;
}
done:
return rc;
error:
detachSHM();
goto done;
}
void iPmdDMNChildProc::syncProcesserInfo()
{
INT32 rc = SDB_OK ;
_syncEvent.reset() ;
while( iPmdProc::isRunning() )
{
rc = attachSHM();
if ( SDB_OK == rc )
{
if ( _procInfo->isInit() &&
PMDDMN_SHM_STAT_DAEMON != _procInfo->stat )
{
_procInfo->pid = ossGetCurrentProcessID() ;
rc = ChildProcessCMD( _procInfo->getCHLCMD() ) ;
if ( SDB_OK != rc )
{
PD_LOG( PDERROR, "Failed to process command(rc=%d)", rc ) ;
}
++( _procInfo->sn ) ;
_procInfo->stat = PMDDMN_SHM_STAT_DAEMON ;
}
detachSHM() ;
}
ossSleep( OSS_ONE_SEC ) ;
}
rc = attachSHM();
if ( SDB_OK == rc )
{
if ( _procInfo->isInit() )
{
PD_LOG( PDEVENT, "stop service..." ) ;
_procInfo->pid = OSS_INVALID_PID ;
_procInfo->stat = PMDDMN_SHM_STAT_DAEMON ;
rc = _procInfo->setDMNCMD( PMDDMN_SHM_CMD_DMN_QUIT ) ;
if ( SDB_OK ==rc )
{
PD_LOG( PDWARNING, "daemon process is not stop!" );
}
}
detachSHM();
}
_syncEvent.signalAll() ;
}
INT32 iPmdDMNChildProc::attachSHM()
{
INT32 rc = SDB_OK;
#if defined ( _WINDOWS )
PD_CHECK( _shmKey != NULL, SDB_OOM, error, PDERROR,
"failed to get key because of malloc failed!" );
#endif
rc = attachSHM( _shmKey );
#if defined ( _WINDOWS )
done:
return rc;
error:
goto done;
#else
return rc;
#endif
}
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);
}
