Ejemplo n.º 1
0
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;
      }
Ejemplo n.º 2
0
/**
 * @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;
}
Ejemplo n.º 3
0
/**
 * @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;
  }