// This function is called when the application exits
// - print out the information relative to the current run
VOID Fini(INT32 code, VOID *v){
//inspect the write set at the end of the execution
WxorXHandler *wxorxHandler = WxorXHandler::getInstance();
//MYINFO("WRITE SET SIZE: %d", wxorxHandler->getWritesSet().size());
//get the execution time
MYPRINT("\n\n\nTotal execution Time: %.2fs", (double)(clock() - tStart)/CLOCKS_PER_SEC);
CLOSELOG();
Report::getInstance()->closeReport();
}
// This function is called when the application exits
VOID Fini(INT32 code, VOID *v){
//DEBUG --- inspect the write set at the end of the execution
WxorXHandler *wxorxHandler = WxorXHandler::getInstance();
MYINFO("WRITE SET SIZE: %d", wxorxHandler->getWritesSet().size());
//DEBUG --- get the execution time
MYINFO("Total execution Time: %.2fs", (double)(clock() - tStart)/CLOCKS_PER_SEC);
CLOSELOG();
Config *config = Config::getInstance();
config->closeReportFile();
}
int main (int argc, char *argv[])
{
bool _headinitialized = false;
bool _arminitialized = false;
bool _headrunning = false;
bool _armrunning = false;
double *_headjointstore = NULL;
double *_armjointstore = NULL;
/// init robot.
INITLOG ();
PRINTLOG ("Log started\n");
FLUSHLOG();
/// head (bus 1).
ValueCanOpenParameters op_par;
memcpy (op_par._destinations, _destinations, sizeof(unsigned char) * CANBUS_MAXCARDS);
op_par._my_address = CANBUS_MY_ADDRESS; /// my address.
op_par._polling_interval = CANBUS_POLLING_INTERVAL; /// thread polling interval [ms].
op_par._timeout = CANBUS_TIMEOUT; /// approx this value times the polling interval [ms].
op_par._njoints = MAX_HEAD_JNTS;
op_par._p = NULL;
if (head.open ((void *)&op_par) < 0)
{
head.close();
ACE_OS::printf ("head driver open failed\n");
}
else
{
// filters out certain messages.
int msg = 20;
head.IOCtl(CMDSetDebugMessageFilter, (void *)&msg);
_headinitialized = true;
}
/// arm (bus 2).
ValueCanOpenParameters op_par2;
op_par2._njoints = MAX_ARM_JNTS;
op_par2._p = PRINTLOG;
memcpy (op_par2._destinations, _destinations, sizeof(unsigned char) * CANBUS_MAXCARDS);
op_par2._my_address = CANBUS_MY_ADDRESS; /// my address.
op_par2._polling_interval = CANBUS_POLLING_INTERVAL; /// thread polling interval [ms].
op_par2._timeout = CANBUS_TIMEOUT; /// approx this value times the polling interval [ms].
if (arm.open ((void *)&op_par2) < 0)
{
arm.close();
ACE_OS::printf ("arm driver open failed\n");
}
else
{
// filters out certain messages.
int msg = 20;
arm.IOCtl(CMDSetDebugMessageFilter, (void *)&msg);
_arminitialized = true;
}
//head.close();
//_headinitialized = false;
if (!_headinitialized && !_arminitialized)
{
ACE_OS::printf ("No device found\n");
return -1;
}
ACE_OS::printf ("The robot is initialized\n");
if (_headinitialized)
{
_headjointstore = new double[MAX_HEAD_JNTS];
ACE_ASSERT (_headjointstore != NULL);
}
if (_arminitialized)
{
_armjointstore = new double[MAX_ARM_JNTS];
ACE_ASSERT (_armjointstore != NULL);
}
ACE_OS::printf ("Running!\n");
int cycle;
for (cycle = 0; ; cycle++)
{
/// read from head.
int ret;
if (_headinitialized)
{
ret = head.IOCtl(CMDGetPositions, _headjointstore);
if (ret != YARP_OK)
ACE_OS::printf ("troubles reading head joint pos\n");
ACE_OS::printf ("head: ");
for (int i = 0; i < MAX_HEAD_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _headjointstore[i]);
}
ACE_OS::printf ("\n");
}
if (_arminitialized)
{
ret = arm.IOCtl(CMDGetPositions, _armjointstore);
if (ret != YARP_OK)
ACE_OS::printf ("troubles reading arm joint pos\n");
ACE_OS::printf ("arm: ");
for (int i = 0; i < MAX_ARM_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _armjointstore[i]);
}
ACE_OS::printf ("\n");
}
Sleep (150);
FLUSHLOG();
}
if (_headinitialized)
{
head.close();
if (_headjointstore != NULL) delete[] _headjointstore;
}
if (_arminitialized)
{
arm.close();
if (_armjointstore != NULL) delete[] _armjointstore;
}
CLOSELOG();
return 0;
}
int main()
{
int ret, status;
char *env_var_ptr;
struct stat gtm_secshrdir_stat;
struct stat gtm_secshr_stat;
char gtm_dist_val[MAX_ENV_VAR_VAL_LEN];
char gtm_tmp_val[MAX_ENV_VAR_VAL_LEN];
char gtm_dbglvl_val[MAX_ENV_VAR_VAL_LEN];
char gtm_secshrdir_path[MAX_ENV_VAR_VAL_LEN];
char gtm_secshrdir_path_display[MAX_ENV_VAR_VAL_LEN];
char gtm_secshr_path[MAX_ENV_VAR_VAL_LEN];
char gtm_secshr_path_display[MAX_ENV_VAR_VAL_LEN];
char gtm_secshr_orig_path[MAX_ENV_VAR_VAL_LEN];
int gtm_tmp_exists = 0;
int gtm_dbglvl_exists = 0;
sigset_t mask;
/* Reset the signal mask (since the one inherited from the invoking process might have signals such as SIGALRM or SIGTERM
* blocked) to let gtmsecshr manage its own signals using sig_init.
*/
sigemptyset(&mask);
sigprocmask(SIG_SETMASK, &mask, NULL);
OPENLOG("GTMSECSHRINIT", LOG_PID | LOG_CONS | LOG_NOWAIT, LOG_USER);
ret = 0; /* start positive */
/* get the ones we need */
if (env_var_ptr = getenv(GTM_DIST)) /* Warning - assignment */
{
if (MAX_ALLOWABLE_LEN < (strlen(env_var_ptr) + STR_LIT_LEN(SUB_PATH_TO_GTMSECSHRDIR)
+ STR_LIT_LEN(GTMSECSHR_BASENAME)))
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRGTMDIST2LONG);
ret = -1;
} else
{
strcpy(gtm_dist_val, env_var_ptr);
/* point the path to the real gtmsecshr - for display purposes only */
strcpy(gtm_secshr_path, env_var_ptr);
strcat(gtm_secshr_path, SUB_PATH_TO_GTMSECSHRDIR);
strcat(gtm_secshr_path, GTMSECSHR_BASENAME);
strsanitize(gtm_secshr_path, gtm_secshr_path_display);
/* point the path to the real gtmsecshrdir */
strcpy(gtm_secshrdir_path, env_var_ptr);
strcat(gtm_secshrdir_path, SUB_PATH_TO_GTMSECSHRDIR);
strsanitize(gtm_secshrdir_path, gtm_secshrdir_path_display);
}
} else
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRNOGTMDIST);
ret = -1;
}
if (env_var_ptr = getenv(GTM_TMP)) /* Warning - assignment */
{
if (MAX_ALLOWABLE_LEN < strlen(env_var_ptr))
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRGTMTMP2LONG);
ret = -1;
} else
{
gtm_tmp_exists = 1;
strcpy(gtm_tmp_val, env_var_ptr);
}
}
if (env_var_ptr = getenv(GTM_DBGLVL)) /* Warning - assignment */
{
if (MAX_ALLOWABLE_LEN < strlen(env_var_ptr))
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRGTMDBGLVL2LONG);
ret = -1;
} else
{
gtm_dbglvl_exists = 1;
strcpy(gtm_dbglvl_val, env_var_ptr);
}
}
if (!ret)
{ /* clear all */
status = gtm_clearenv();
if (status)
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRCLEARENVFAILED);
ret = -1;
}
/* add the ones we need */
status = gtm_setenv(GTM_DIST, gtm_dist_val, OVERWRITE);
if (status)
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRSETGTMDISTFAILED);
ret = -1;
}
if (gtm_tmp_exists)
{
status = gtm_setenv(GTM_TMP, gtm_tmp_val, OVERWRITE);
if (status)
{
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRSETGTMTMPFAILED);
ret = -1;
}
}
}
if (!ret)
{ /* go to root */
if (-1 == CHDIR(gtm_secshrdir_path))
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRCHDIRFAILED, gtm_secshrdir_path_display, errno);
else if (-1 == Stat(REL_PATH_TO_CURDIR, >m_secshrdir_stat))
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRSTATFAILED, gtm_secshrdir_path_display, errno);
else if (ROOTUID != gtm_secshrdir_stat.st_uid)
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRNOTOWNEDBYROOT, gtm_secshrdir_path_display);
else if (gtm_secshrdir_stat.st_mode & 0277)
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRPERMINCRCT, gtm_secshrdir_path_display,
gtm_secshrdir_stat.st_mode & 0777);
else if (-1 == Stat(REL_PATH_TO_GTMSECSHR, >m_secshr_stat))
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRSTATFAILED, gtm_secshr_path_display, errno);
else if (ROOTUID != gtm_secshr_stat.st_uid)
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRNOTOWNEDBYROOT, gtm_secshr_path_display);
else if (gtm_secshr_stat.st_mode & 022)
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRWRITABLE, gtm_secshr_path_display);
else if (!(gtm_secshr_stat.st_mode & 04000))
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRNOTSETUID, gtm_secshr_path_display);
else if (-1 == setuid(ROOTUID))
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHRSETUIDFAILED);
else
{ /* call the real gtmsecshr, but have ps display the original gtmsecshr location */
strcpy(gtm_secshr_orig_path, gtm_dist_val);
strcat(gtm_secshr_orig_path, GTMSECSHR_BASENAME);
ret = execl(REL_PATH_TO_GTMSECSHR, gtm_secshr_orig_path, NULL);
if (-1 == ret)
SYSLOG(LOG_USER | LOG_INFO, ERR_SECSHREXECLFAILED, gtm_secshr_path_display);
}
}
CLOSELOG();
return ret;
}
int main (int argc, char *argv[])
{
bool _headinitialized = false;
bool _arminitialized = false;
bool _headrunning = false;
bool _armrunning = false;
double *_headjointstore = NULL;
double *_armjointstore = NULL;
/// init robot.
INITLOG ();
PRINTLOG ("Log started\n");
FLUSHLOG();
char *root = GetYarpRoot();
char path[512];
ACE_OS::sprintf (path, "%s/%s\0", root, ConfigFilePath);
// initialize head controller on bus 1.
YARPRobotcubHeadParameters parameters;
parameters.load (YARPString(path), YARPString(HEAD_INI_FILE));
parameters._message_filter = 20;
parameters._p = NULL; //xprintf;
int ret = head.initialize(parameters);
if (ret != YARP_OK)
{
_headinitialized = false;
ACE_OS::printf ("Can't start the interface with the robot head, please make sure the hardware and control cards are on.\nThe ini file was: %s%s", path, HEAD_INI_FILE);
}
else
{
_headinitialized = true;
_headrunning = false;
ACE_ASSERT (head.nj() == MAX_HEAD_JNTS); // not other size is allowed.
}
// initialize arm controller on bus 2.
YARPRobotcubArmParameters aparameters;
aparameters.load (YARPString(path), YARPString(ARM_INI_FILE));
aparameters._message_filter = 20;
aparameters._p = (int (*) (char *, ...)) PRINTLOG;
ret = arm.initialize(aparameters);
if (ret != YARP_OK)
{
_arminitialized = false;
ACE_OS::printf ("Can't start the interface with the robot arm/hand, please make sure the hardware and control cards are on.\nThe ini file was: %s%s", path, ARM_INI_FILE);
}
else
{
_arminitialized = true;
_armrunning = false;
ACE_ASSERT (arm.nj() == MAX_ARM_JNTS);
}
if (!_headinitialized && !_arminitialized)
{
ACE_OS::printf ("No device found\n");
return -1;
}
ACE_OS::printf ("The robot is initialized\n");
if (_headinitialized)
{
_headjointstore = new double[head.nj()];
ACE_ASSERT (_headjointstore != NULL);
}
if (_arminitialized)
{
_armjointstore = new double[arm.nj()];
ACE_ASSERT (_armjointstore != NULL);
}
ACE_OS::printf ("Running!\n");
int cycle;
for (cycle = 0; ; cycle++)
{
if (_headinitialized)
{
head.getPositions (_headjointstore);
ACE_OS::printf ("head: ");
for (int i = 0; i < MAX_HEAD_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _headjointstore[i]);
}
ACE_OS::printf ("\n");
}
if (_arminitialized)
{
arm.getPositions (_armjointstore);
ACE_OS::printf ("arm: ");
for (int i = 0; i < MAX_ARM_JNTS; i++)
{
ACE_OS::printf ("%.2f ", _armjointstore[i]);
}
ACE_OS::printf ("\n");
}
Sleep (150);
FLUSHLOG();
}
/// close devices.
///
if (_headinitialized)
{
head.idleMode ();
_headrunning = false;
head.uninitialize ();
_headinitialized = false;
if (_headjointstore != NULL) delete[] _headjointstore;
}
if (_arminitialized)
{
arm.idleMode ();
_armrunning = false;
arm.uninitialize ();
_arminitialized = false;
if (_armjointstore != NULL) delete[] _armjointstore;
}
CLOSELOG();
return 0;
}
