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) {
(void)keyp;
(void)paramp;
urosAssert(urosStringNotEmpty(keyp));
urosAssert(paramp != NULL);
/* Unknown parameter name.*/
return UROS_ERR_BADPARAM;
}
/**
* @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;
}