/**
* @brief Creates all the thread pool threads.
* @details The thread pool is filled with new worker threads. The fucntion
* waits until all the worker threads are ready.
* @see urosThreadPoolJoinAll()
* @see urosThreadPoolClean()
*
* @pre The thread pools has no allocated worker threads.
* @post All the worker threads are ready.
*
* @param[in,out] poolp
* Pointer to an initialized @p UrosThreadPool object.
* @return
* Error code.
*/
uros_err_t urosThreadPoolCreateAll(UrosThreadPool *poolp) {
uros_cnt_t i;
uros_err_t err;
urosAssert(poolp != NULL);
urosAssert(poolp->stackPoolp != NULL);
urosAssert(poolp->threadsp != NULL);
urosAssert(poolp->size > 0);
/* Create the threads from the memory pool.*/
poolp->exitFlag = UROS_FALSE;
for (i = 0; i < poolp->size; ++i) {
err = urosThreadCreateFromMemPool(&poolp->threadsp[i], poolp->namep,
poolp->priority,
(uros_proc_f)urosThreadPoolWorkerThread,
(void*)poolp, poolp->stackPoolp);
urosAssert(err != UROS_ERR_NOMEM);
urosError(err != UROS_OK, return err,
("Error [%s] while creating thread %d (of %d) in pool\n",
strerror(err), i, poolp->size));
}
/* Wait until all the threads are ready.*/
urosMutexLock(&poolp->readyMtx);
while (poolp->readyCnt < poolp->size) {
urosCondVarWait(&poolp->readyCond, &poolp->readyMtx);
}
urosMutexUnlock(&poolp->readyMtx);
return UROS_OK;
}
/**
* brief Joins all the worker threads.
* @details Waits for all the worker threads to terminate.
* @see urosThreadPoolClean()
*
* @pre The worker threads are created.
* @post The worker threads have terminated and cannot be started again.
*
* @param[in,out] poolp
* Pointer to an initialized @p UrosThreadPool object.
* @return
* Error code.
*/
uros_err_t urosThreadPoolJoinAll(UrosThreadPool *poolp) {
uros_cnt_t i;
urosAssert(poolp != NULL);
urosAssert(poolp->stackPoolp != NULL);
urosAssert(!(poolp->size > 0) || (poolp->threadsp != NULL));
/* Wait for all the running threads to terminate.*/
urosMutexLock(&poolp->readyMtx);
while (poolp->readyCnt < poolp->size) {
urosCondVarWait(&poolp->readyCond, &poolp->readyMtx);
}
urosMutexUnlock(&poolp->readyMtx);
/* Restart all the threads with the exit flag set.*/
urosMutexLock(&poolp->busyMtx);
poolp->exitFlag = UROS_TRUE;
poolp->busyCnt = poolp->size;
urosCondVarBroadcast(&poolp->busyCond);
urosMutexUnlock(&poolp->busyMtx);
/* Wait until all the threads have exited.*/
urosMutexLock(&poolp->readyMtx);
while (poolp->readyCnt < poolp->size) {
urosCondVarWait(&poolp->readyCond, &poolp->readyMtx);
}
urosMutexUnlock(&poolp->readyMtx);
for (i = 0; i < poolp->size; ++i) {
urosThreadJoin(poolp->threadsp[i]);
poolp->threadsp[i] = UROS_NULL_THREADID;
}
return UROS_OK;
}
void app_print_thread_state(pthread_t threadId) {
char namebuf[32];
clockid_t clockId;
struct timespec time;
int err; (void)err;
err = pthread_getcpuclockid(threadId, &clockId);
urosAssert(err == 0);
err = clock_gettime(clockId, &time);
urosAssert(err == 0);
memset(namebuf, 0, sizeof(namebuf));
err = pthread_getname_np(threadId, namebuf, 31);
urosAssert(err == 0);
namebuf[31] = 0;
#if PTHREAD_GETNAME_NP_NEWLINE
if (strlen(namebuf) >= 2) {
namebuf[strlen(namebuf) - 2] = 0;
}
#endif
printf("%lu %lu.%9.9lu %s\n",
(unsigned long)threadId,
(unsigned long)time.tv_sec, (unsigned long)time.tv_nsec,
namebuf);
}
static void uros_nodeconfig_readstring(UrosString *strp, FILE *fp) {
size_t length, n;
char *datap;
urosAssert(strp != NULL);
urosAssert(fp != NULL);
/* Read the string length.*/
n = fread(&length, sizeof(size_t), 1, fp);
urosError(n < 1, return,
("Cannot read string length at offset 0x%.8lX\n", ftell(fp)));
/* Read the string data.*/
datap = (char*)urosAlloc(NULL, length);
urosAssert(datap != NULL);
n = fread(datap, length, 1, fp);
urosError(n < 1, return,
("Cannot read string data (%u bytes) at offset 0x%.8lX\n",
(unsigned)length, ftell(fp)));
/* Assign the data back.*/
strp->length = length;
strp->datap = datap;
}
void rosout_post(UrosString *strp, uros_bool_t costant, uint8_t level,
const char *fileszp, int line, const char *funcp) {
static uint32_t seq = 0;
struct msg__rosgraph_msgs__Log *msgp;
urosAssert(urosStringIsValid(strp));
msgp = urosNew(NULL, struct msg__rosgraph_msgs__Log);
urosAssert(msgp != NULL);
init_msg__rosgraph_msgs__Log(msgp);
msgp->header.frame_id = urosStringAssignZ(costant ? "1" : "0");
msgp->header.seq = seq++;
msgp->header.stamp.sec = urosGetTimestampMsec();
msgp->header.stamp.nsec = (msgp->header.stamp.sec % 1000) * 1000000;
msgp->header.stamp.sec /= 1000;
msgp->level = level;
msgp->name = urosNode.config.nodeName;
msgp->msg = *strp;
msgp->file = urosStringAssignZ(fileszp);
msgp->function = urosStringAssignZ(funcp);
msgp->line = line;
fifo_enqueue(&rosoutQueue, (void *)msgp);
}
/**
* @brief Shutdown callback function.
* @details This callback function notifies the user that a @p shutdown()
* XMLRPC call was issued by the Master node, and has to be handled.
*
* @param[in] msgp
* Pointer to a string which explains the reason why it is asked to be
* shutdown.
* @return
* Error code.
*/
uros_err_t urosUserShutdown(const UrosString *msgp) {
static UrosNodeStatus *const stp = &urosNode.status;
(void)msgp;
(void)stp;
#if UROS_USE_ASSERT
urosAssert(msgp != NULL);
urosMutexLock(&stp->stateLock);
urosAssert(stp->state == UROS_NODE_SHUTDOWN);
urosMutexUnlock(&stp->stateLock);
#endif
/* Send a dummy getPid() request, to unlock XMLRPC listener and pool.*/
{
UrosRpcResponse res;
urosRpcResponseObjectInit(&res);
urosRpcCallGetPid(
&urosNode.config.xmlrpcAddr,
&urosNode.config.nodeName,
&res
);
urosRpcResponseClean(&res);
}
return UROS_OK;
}
/**
* @brief Initializes a thread pool
* @details The thread pool is initialized with the related attributes, and any
* private members.
*
* @pre The thread pool is not initialized.
*
* @param[in,out] poolp
* Pointer to an allocated @p UrosThreadPool object.
* @param[in] stackpoolp
* Pointer to an initialized memory pool.
* @param[in] routine
* Routine of the worker threads.
* @param[in] namep
* Pointer to the default thread name, valid for the whole the thread
* life. Null-terminated string.
* @param[in] priority
* Default worker thread priority.
* @return
* Error code.
*/
uros_err_t urosThreadPoolObjectInit(UrosThreadPool *poolp,
UrosMemPool *stackpoolp,
uros_proc_f routine,
const char *namep,
uros_prio_t priority) {
uros_cnt_t i;
urosAssert(poolp != NULL);
urosAssert(stackpoolp != NULL);
urosAssert(routine != NULL);
poolp->stackPoolp = stackpoolp;
poolp->size = urosMemPoolNumFree(stackpoolp);
urosAssert(poolp->size > 0);
poolp->argp = NULL;
poolp->routine = routine;
poolp->namep = namep;
poolp->priority = priority;
poolp->threadsp = urosArrayNew(NULL, poolp->size, UrosThreadId);
if (poolp->threadsp != NULL) {
for (i = 0; i < poolp->size; ++i) {
poolp->threadsp[i] = UROS_NULL_THREADID;
}
}
poolp->readyCnt = 0;
poolp->busyCnt = 0;
urosMutexObjectInit(&poolp->readyMtx);
urosMutexObjectInit(&poolp->busyMtx);
urosCondVarObjectInit(&poolp->readyCond);
urosCondVarObjectInit(&poolp->busyCond);
poolp->exitFlag = UROS_FALSE;
return (poolp->threadsp != NULL) ? UROS_OK : UROS_ERR_NOMEM;
}
/**
* @brief Unlocks a mutex.
*
* @pre The mutex is locked.
* @post The mutex is unlocked.
*
* @param[in,out] mtxp
* Pointer to a locked @p UrosMutex object.
*/
void uros_lld_mutex_unlock(UrosMutex *mtxp) {
UrosMutex *unlocked;
(void)unlocked;
(void)mtxp;
urosAssert(mtxp != NULL);
unlocked = chMtxUnlock();
urosAssert(unlocked == mtxp);
}
static void uros_nodeconfig_readaddr(UrosAddr *addrp, FILE *fp) {
size_t n;
urosAssert(addrp != NULL);
urosAssert(fp != NULL);
n = fread(addrp, sizeof(UrosAddr), 1, fp);
urosError(n < 1, return,
("Cannot read connection address at offset 0x%.8lX\n", ftell(fp)));
}
/**
* @brief Waits for a condvar signal.
* @details Waits until the condvar is signalled.
* @note This procedure must be called within a lock zone guarded by a
* mutex, shared by the waiting and the signalling thread.
*
* @param[in,out] cvp
* Pointer to an initialized @p UrosCondVar object.
* @param[in,out] mtxp
* Pointer to the mutex guarding this condvar.
*/
void uros_lld_condvar_wait(UrosCondVar *cvp, UrosMutex *mtxp) {
urosAssert(cvp != NULL);
#if UROS_THREADING_C_USE_ASSERT != UROS_FALSE
chSysLock();
urosAssert(mtxp == currp->p_mtxlist);
chSysUnlock();
#endif
(void)mtxp;
chCondWait(cvp);
}
/**
* @brief Updates a subscribed ROS parameter locally.
* @details This callback function notifies the user that the value of a
* subscribed ROS parameter has changed.
*
* @param[in] keyp
* Pointer to the parameter name string.
* @param[in] paramp
* Pointer to the parameter value.
* @return
* Error code.
*/
uros_err_t urosUserParamUpdate(const UrosString *keyp,
const UrosRpcParam *paramp) {
(void)keyp;
(void)paramp;
urosAssert(urosStringNotEmpty(keyp));
urosAssert(paramp != NULL);
/* Unknown parameter name.*/
return UROS_ERR_BADPARAM;
}
static void uros_nodeconfig_writeaddr(const UrosAddr *addrp, FILE *fp) {
size_t n;
urosAssert(addrp != NULL);
urosAssert(fp != NULL);
n = fwrite(addrp, sizeof(UrosAddr), 1, fp);
urosError(n < 1, return,
("Cannot write connection address ("UROS_ADDRFMT
") at offset 0x%.8lX\n",
UROS_ADDRARG(addrp), ftell(fp)));
}
void fifo_init(fifo_t *queuep, unsigned length) {
urosAssert(queuep != NULL);
urosAssert(length > 0);
urosSemObjectInit(&queuep->freeSem, length);
urosSemObjectInit(&queuep->usedSem, 0);
queuep->length = length;
queuep->head = 0;
queuep->tail = 0;
urosMutexObjectInit(&queuep->slotsMtx);
queuep->slots = urosArrayNew(NULL, length, void *);
urosAssert(queuep->slots != NULL);
}
void fifo_enqueue(fifo_t *queuep, void *msgp) {
urosAssert(queuep != NULL);
urosAssert(msgp != NULL);
urosSemWait(&queuep->freeSem);
urosMutexLock(&queuep->slotsMtx);
queuep->slots[queuep->tail] = msgp;
if (++queuep->tail >= queuep->length) {
queuep->tail = 0;
}
urosMutexUnlock(&queuep->slotsMtx);
urosSemSignal(&queuep->usedSem);
}
/**
* @brief Loads node configuration.
* @details Any previously allocated data is freed, then the configuration is
* loaded from a static non-volatile memory chunk.
* @see uros_lld_nodeconfig_load()
*
* @pre The related @p UrosNode is initialized.
*
* @param[in,out] cfgp
* Pointer to the target configuration descriptor.
*/
void urosUserNodeConfigLoad(UrosNodeConfig *cfgp) {
FILE *fp;
urosAssert(cfgp != NULL);
/* Clean any allocated variables.*/
urosStringClean(&cfgp->nodeName);
urosStringClean(&cfgp->xmlrpcUri);
urosStringClean(&cfgp->tcprosUri);
urosStringClean(&cfgp->masterUri);
/* Read from file.*/
fp = fopen(UROS_NODECONFIG_FILENAME, "rb");
if (fp == NULL) {
/* File not found, load default values and save them.*/
urosError(fp == NULL, UROS_NOP,
("Cannot open file ["UROS_NODECONFIG_FILENAME"] for reading\n"
" (The default configuration will be written there if possible)\n"));
urosNodeConfigLoadDefaults(cfgp);
urosUserNodeConfigSave(cfgp);
return;
}
uros_nodeconfig_readstring(&cfgp->nodeName, fp);
uros_nodeconfig_readaddr (&cfgp->xmlrpcAddr, fp);
uros_nodeconfig_readstring(&cfgp->xmlrpcUri, fp);
uros_nodeconfig_readaddr (&cfgp->tcprosAddr, fp);
uros_nodeconfig_readstring(&cfgp->tcprosUri, fp);
uros_nodeconfig_readaddr (&cfgp->masterAddr, fp);
uros_nodeconfig_readstring(&cfgp->masterUri, fp);
fclose(fp);
}
turtle_t *turtle_spawn(const UrosString *namep,
float x, float y, float theta) {
static const char *const posend = "/pose";
static const char *const colsenend = "/color_sensor";
static const char *const velend = "/command_velocity";
static const char *const setpenend = "/set_pen";
static const char *const telabsend = "/teleport_absolute";
static const char *const telrelend = "/teleport_relative";
turtle_t *turtlep;
unsigned i, numAlive;
uros_err_t err;
urosAssert(urosStringNotEmpty(namep));
/* Check if the turtle can be spawned.*/
urosMutexLock(&turtleCanSpawnLock);
if (!turtleCanSpawn) {
urosMutexUnlock(&turtleCanSpawnLock);
return NULL;
}
urosMutexUnlock(&turtleCanSpawnLock);
/* Fill an empty slot.*/
for (turtlep = NULL, numAlive = 0; turtlep == NULL;) {
for (i = 0, turtlep = turtles; i < MAX_TURTLES; ++turtlep, ++i) {
urosMutexLock(&turtlep->lock);
if (turtlep->status == TURTLE_ALIVE) {
urosError(0 == urosStringCmp(&turtlep->name, namep),
{ urosMutexUnlock(&turtlep->lock); return NULL; },
("A turtle named [%.*s] is alive\n", UROS_STRARG(namep)));
++numAlive;
}
/**
* @brief Updates a subscribed ROS parameter locally.
* @details This callback function notifies the user that the value of a
* subscribed ROS parameter has changed.
*
* @param[in] keyp
* Pointer to the parameter name string.
* @param[in] paramp
* Pointer to the parameter value.
* @return
* Error code.
*/
uros_err_t urosUserParamUpdate(const UrosString *keyp,
const UrosRpcParam *paramp) {
static const char hex[16] = {
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
};
(void)keyp;
(void)paramp;
urosAssert(urosStringNotEmpty(keyp));
urosAssert(paramp != NULL);
if (0 == urosStringCmp(keyp, &rateparamname)) {
urosAssert(paramp->class == UROS_RPCP_INT);
benchmark.rate = (uint32_t)paramp->value.int32;
return UROS_OK;
}
static void uros_nodeconfig_writestring(const UrosString *strp, FILE *fp) {
size_t n;
urosAssert(strp != NULL);
urosAssert(fp != NULL);
/* Write the string length.*/
n = fwrite(&strp->length, sizeof(size_t), 1, fp);
urosError(n < 1, return,
("Cannot write string length (%u bytes) at offset 0x%.8lX\n",
(unsigned)strp->length, ftell(fp)));
/* Write the string data.*/
n = fwrite(strp->datap, strp->length, 1, fp);
urosError(n < 1, return,
("Cannot write string data [%.*s] at offset 0x%.8lX\n",
UROS_STRARG(strp), ftell(fp)));
}
/**
* @brief Registers all the subscribed parameters to the Master node.
* @note Should be called at node initialization.
*
* @return Error code.
*/
uros_err_t urosUserSubscribeParams(void) {
static const UrosNodeConfig *const cfgp = &urosNode.config;
UrosRpcParam paramval;
UrosRpcResponse res;
uros_err_t err; (void)err;
urosRpcResponseObjectInit(&res);
urosRpcParamObjectInit(¶mval, UROS_RPCP_INT);
/* Output rate (packets/s).*/
err = urosRpcCallHasParam(&cfgp->masterAddr, &cfgp->nodeName,
&rateparamname, &res);
urosAssert(err == UROS_OK);
if (!res.valuep->value.boolean) {
urosRpcResponseClean(&res);
paramval.value.int32 = 1;
err = urosRpcCallSetParam(&cfgp->masterAddr, &cfgp->nodeName,
&rateparamname, ¶mval, &res);
urosAssert(err == UROS_OK);
}
urosRpcResponseClean(&res);
urosNodeSubscribeParam(&rateparamname);
/* Packet size (string length).*/
err = urosRpcCallHasParam(&cfgp->masterAddr, &cfgp->nodeName,
&sizeparamname, &res);
urosAssert(err == UROS_OK);
if (!res.valuep->value.boolean) {
urosRpcResponseClean(&res);
paramval.value.int32 = 0;
err = urosRpcCallSetParam(&cfgp->masterAddr, &cfgp->nodeName,
&sizeparamname, ¶mval, &res);
urosAssert(err == UROS_OK);
}
urosRpcResponseClean(&res);
urosNodeSubscribeParam(&sizeparamname);
urosRpcParamClean(¶mval, UROS_TRUE);
return UROS_OK;
}
/**
* @brief Gets the semaphore value.
* @details Reads the current semaphore value.
* @warning Thread-safe, but the value may not be consistent after a call to
* this function.
*
* @param[in] semp
* Pointer to an initialized @p UrosSem object.
* @return
* Current semaphore count.
*/
uros_cnt_t uros_lld_sem_value(UrosSem *semp) {
uros_cnt_t value;
urosAssert(semp != NULL);
chSysLock();
value = chSemGetCounterI(semp);
chSysUnlock();
return value;
}
/**
* @brief Worker thread.
* @details This is the actual thread function created by @p urosThreadPoolCreateAll().
* After creation, it waits until the user starts this thread by
* calling @p urosThreadPoolStartWorker(), when this is the first
* ready thread selected.
*
* After executing the routine defined with urosThreadPoolObjectInit(),
* this thread goes back into the waiting state.
*
* To exit from this thread, it first has to be put back into the
* waiting state. Then, the @p exitFlag of the pool must be set to
* @p true, so that by calling @p urosThreadPoolStartWorker() again
* on this thread, the latter knows it must exit and not execute the
* user routine.
*
* @param[in] poolp
* Pointer to the @p UrosThreadPool which created this thread.
* @return
* Error code returned by the user routine.
*/
uros_err_t urosThreadPoolWorkerThread(UrosThreadPool *poolp) {
uros_bool_t exit;
uros_err_t err;
void *argp;
urosAssert(poolp != NULL);
urosAssert(poolp->routine != NULL);
/* Notify the creation of a thread.*/
urosMutexLock(&poolp->readyMtx);
++poolp->readyCnt;
urosCondVarSignal(&poolp->readyCond);
urosMutexUnlock(&poolp->readyMtx);
do {
/* Wait for the user routine to start with the provided argument pointer.*/
urosMutexLock(&poolp->busyMtx);
while (poolp->busyCnt == 0) {
urosCondVarWait(&poolp->busyCond, &poolp->busyMtx);
}
--poolp->busyCnt;
argp = poolp->argp;
exit = poolp->exitFlag;
urosMutexUnlock(&poolp->busyMtx);
if (!exit) {
/* Launch the wrapped user thread routine.*/
urosAssert(poolp->routine != NULL);
err = poolp->routine(argp);
} else {
err = UROS_OK;
}
/* Notify the release of this thread.*/
urosMutexLock(&poolp->readyMtx);
++poolp->readyCnt;
urosCondVarSignal(&poolp->readyCond);
urosMutexUnlock(&poolp->readyMtx);
} while (!exit);
return err;
}
/**
* @brief Starts a worker thread.
* @details Waits until there is a ready worker thread in the pool, selects it,
* passes the argument provided by the user to it, and lets it run.
*
* @pre The worker threads are created.
* @post The worker thread is running the routine assigne to the pool, with
* the argument provided by the user.
*
* @param[in,out] poolp
* Pointer to an initialized @p UrosThreadPool object.
* @param[in] argp
* Argument passed to the thread routine.
* @return
* Error code.
*/
uros_err_t urosThreadPoolStartWorker(UrosThreadPool *poolp, void *argp) {
urosAssert(poolp != NULL);
urosAssert(poolp->stackPoolp != NULL);
/* Wait for a free thread.*/
urosMutexLock(&poolp->readyMtx);
while (poolp->readyCnt == 0) {
urosCondVarWait(&poolp->readyCond, &poolp->readyMtx);
}
--poolp->readyCnt;
urosMutexUnlock(&poolp->readyMtx);
/* Start a thread with the given argument pointer.*/
urosMutexLock(&poolp->busyMtx);
poolp->argp = argp;
++poolp->busyCnt;
urosCondVarSignal(&poolp->busyCond);
urosMutexUnlock(&poolp->busyMtx);
return UROS_OK;
}
/**
* @brief Loads node configuration.
* @details Any previously allocated data is freed, then the configuration is
* loaded from a static non-volatile memory chunk.
* @see uros_lld_nodeconfig_load()
*
* @pre The related @p UrosNode is initialized.
*
* @param[in,out] cfgp
* Pointer to the target configuration descriptor.
*/
void urosUserNodeConfigLoad(UrosNodeConfig *cfgp) {
urosAssert(cfgp != NULL);
/* Clean any allocated variables.*/
urosStringClean(&cfgp->nodeName);
urosStringClean(&cfgp->xmlrpcUri);
urosStringClean(&cfgp->tcprosUri);
urosStringClean(&cfgp->masterUri);
/* TODO: Load from EEPROM.*/
urosNodeConfigLoadDefaults(cfgp);
}
void app_print_cpu_state(void) {
FILE *statfp = NULL;
cpucnt_t curCpu, oldCpu;
unsigned long temp;
int n; (void)n;
statfp = fopen("/proc/stat", "rt");
urosAssert(statfp != NULL);
n = fscanf(statfp, "%*s %lu %lu %lu %lu %lu %lu %lu ",
&curCpu.user, &curCpu.nice, &curCpu.system, &curCpu.idle,
&curCpu.iowait, &curCpu.irq, &curCpu.softirq);
urosAssert(n == 7);
do {
n = fscanf(statfp, "%lu ", &temp);
if (n == 1) {
curCpu.misc += temp;
}
} while (n == 1);
fclose(statfp);
urosMutexLock(&benchmark.lock);
oldCpu = benchmark.oldCpu = benchmark.curCpu;
benchmark.curCpu = curCpu;
urosMutexUnlock(&benchmark.lock);
printf(
"CPU: user: %lu nice: %lu sys: %lu idle: %lu "
"iow: %lu irq: %lu swirq: %lu misc: %lu\n",
curCpu.user - oldCpu.user,
curCpu.nice - oldCpu.nice,
curCpu.system - oldCpu.system,
curCpu.idle - oldCpu.idle,
curCpu.iowait - oldCpu.iowait,
curCpu.irq - oldCpu.irq,
curCpu.softirq - oldCpu.softirq,
curCpu.misc - oldCpu.misc
);
}
/**
* @brief Creates a thread allocating the stack on the deafult heap.
*
* @pre The stack is big enough to avoid stack overflow.
*
* @param[out] idp
* Pointer to the @p UrosThreadId where the created thread id is
* stored.
* @param[in] namep
* Pointer to the default thread name, valid for the whole the thread
* life. Null-terminated string.
* @param[in] priority
* Thread priority.
* @param[in] routine
* Thread routine, entry point of the thread program.
* @param[in] argp
* Argument passed to the thread routine. Can be @p NULL.
* @param[in] stacksize
* Stack size, in bytes.
* @return
* Error code.
*/
uros_err_t uros_lld_thread_createfromheap(UrosThreadId *idp, const char *namep,
uros_prio_t priority,
uros_proc_f routine, void *argp,
size_t stacksize) {
urosAssert(idp != NULL);
urosAssert(routine != NULL);
urosAssert(stacksize > 0);
*idp = chThdCreateFromHeap(NULL, stacksize, priority, (void*)routine, argp);
if (*idp != NULL) {
#if CH_USE_REGISTRY
/* Set the thread name.*/
chSysLock();
(*idp)->p_name = namep;
chSysUnlock();
#else
(void)namep;
#endif
return UROS_OK;
}
return UROS_ERR_NOMEM;
}
/**
* @brief Creates a thread allocating the stack from a memory pool.
* @details Creates a new thread. The memory chunk for the stack is allocated
* from a memory pool.
* @warning The thread creation is suspended until there is a free slot in the
* memory pool.
*
* @pre The stack is big enough to avoid stack overflow.
*
* @param[out] idp
* Pointer to the @p UrosThreadId where the created thread id is
* stored.
* @param[in] namep
* Pointer to the default thread name, valid for the whole the thread
* life. Null-terminated string.
* @param[in] priority
* Thread priority.
* @param[in] routine
* Thread routine, entry point of the thread program.
* @param[in] argp
* Argument passed to the thread routine. Can be @p NULL.
* @param[in] mempoolp
Pointer to the memory pool where to allocate a stack.
* @return
* Error code.
*/
uros_err_t uros_lld_thread_createfrommempool(UrosThreadId *idp, const char *namep,
uros_prio_t priority,
uros_proc_f routine, void *argp,
UrosMemPool *mempoolp) {
urosAssert(idp != NULL);
urosAssert(routine != NULL);
urosAssert(mempoolp != NULL);
*idp = chThdCreateFromMemoryPool(mempoolp, priority, routine, argp);
if (*idp != NULL) {
#if CH_USE_REGISTRY
/* Set the thread name.*/
chSysLock();
(*idp)->p_name = namep;
chSysUnlock();
#else
(void)namep;
#endif
return UROS_OK;
}
return UROS_ERR_NOMEM;
}
void *fifo_dequeue(fifo_t *queuep) {
void *msgp;
urosAssert(queuep != NULL);
urosSemWait(&queuep->usedSem);
urosMutexLock(&queuep->slotsMtx);
msgp = queuep->slots[queuep->head];
if (++queuep->head >= queuep->length) {
queuep->head = 0;
}
urosMutexUnlock(&queuep->slotsMtx);
urosSemSignal(&queuep->freeSem);
return msgp;
}
/**
* @brief Cleans a thread pool.
* @details Deinitializes private members, and deallocates any allocated
* objects.
* @note The related memory pool is not deallocated nor uninitialized.
*
* @pre The thread pool is initialized.
* @pre No worker thread is running.
* @post The thread pool is not initialized, call @p urosThreadPoolObjectInit()
* to use it again.
*
* @param[in,out] poolp
* Pointer to an initialized memory pool.
*/
void urosThreadPoolClean(UrosThreadPool *poolp) {
urosAssert(poolp != NULL);
poolp->stackPoolp = NULL;
poolp->size = 0;
poolp->argp = NULL;
poolp->routine = NULL;
urosFree(poolp->threadsp);
poolp->readyCnt = 0;
poolp->busyCnt = 0;
urosMutexClean(&poolp->readyMtx);
urosMutexClean(&poolp->busyMtx);
urosCondVarClean(&poolp->readyCond);
urosCondVarClean(&poolp->busyCond);
poolp->exitFlag = UROS_FALSE;
}
void app_print_cpu_usage(void) {
struct timespec time;
int err; (void)err;
#if defined(CLOCK_PROCESS_CPUTIME_ID)
err = clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time);
#elif defined(CLOCK_VIRTUAL)
err = clock_gettime(CLOCK_VIRTUAL, &time);
#else
#error "process' clock_gettime() not available"
#endif
urosAssert(err == 0);
printf("USER: %lu.%9.9lu\n",
(unsigned long)time.tv_sec, (unsigned long)time.tv_nsec);
}
uros_err_t turtle_brain_thread(turtle_t *turtlep) {
struct msg__turtlesim__Pose *posep;
urosAssert(turtlep != NULL);
/* Simple integration.*/
posep = (struct msg__turtlesim__Pose *)&turtlep->pose;
urosMutexLock(&turtlep->lock);
while (turtlep->status == TURTLE_ALIVE) {
/* Execute commands until their deadline.*/
if (turtlep->countdown > 0) {
--turtlep->countdown;
posep->x += (float)cos(posep->theta) * posep->linear_velocity
* (0.001f * TURTLE_THREAD_PERIOD_MS);
posep->y += (float)sin(posep->theta) * posep->linear_velocity
* (0.001f * TURTLE_THREAD_PERIOD_MS);
posep->theta += posep->angular_velocity
* (0.001f * TURTLE_THREAD_PERIOD_MS);
/* Clamp values.*/
if (posep->x < 0 || posep->x > SANDBOX_WIDTH ||
posep->y < 0 || posep->y > SANDBOX_WIDTH) {
static const UrosString msg = { 19, "Turtle hit the wall" };
rosout_warn((UrosString*)&msg, UROS_TRUE);
}
posep->x = min(max(0, posep->x), SANDBOX_WIDTH);
posep->y = min(max(0, posep->y), SANDBOX_HEIGHT);
while (posep->theta < 0) { posep->theta += _2PI; }
while (posep->theta >= _2PI) { posep->theta -= _2PI; }
} else {
posep->linear_velocity = 0;
posep->angular_velocity = 0;
}
urosMutexUnlock(&turtlep->lock);
urosThreadSleepMsec(TURTLE_THREAD_PERIOD_MS);
urosMutexLock(&turtlep->lock);
}
turtle_unref(turtlep);
urosMutexUnlock(&turtlep->lock);
return UROS_OK;
}
