/** * @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; }