urgAutoCapture.cpp

/*
  URG に自動データ取得機能を追加
  Satofumi KAMIMURA
  $Id$
*/

#include "urgAutoCapture.h"
#include <stdio.h>

#if !HAVE_CONFIG_H || HAVE_LIBSDL
#include <SDL.h>


int URGAutoCapture::captureThread(void* args) {
  URGAutoCapture* obj = (URGAutoCapture*)args;

  bool active = true;
  do {
    obj->lock();
    bool obj_initialized = obj->initialized;
    obj->unlock();

    if (obj_initialized) {
      unsigned long raw_timestamp = 0;
      int back_index = 1 - obj->active_index;
      obj->capture_buffer[back_index].ret_value =
        obj->URGManualCapture::capture(obj->capture_buffer[back_index].length,
                                       obj->next_first, obj->next_last,
                                       obj->next_group, *(obj->urg_params),
                                       (obj->enableTimestamp)
                                       ? &raw_timestamp : NULL);
      obj->capture_buffer[back_index].first = obj->next_first;
      obj->capture_buffer[back_index].last = obj->next_last;
      obj->capture_buffer[back_index].group = obj->next_group;
      obj->capture_buffer[back_index].raw_timestamp = raw_timestamp;
      obj->capture_buffer[back_index].captured = true;
      obj->capture_buffer[back_index].capture_times =
        obj->URGManualCapture::getCaptureTimes();

      // バッファの切り替え
      obj->capture_buffer[obj->active_index].captured = false;
      obj->active_index = back_index;
    } else {
      SDL_Delay(WaitMsec);
    }
    obj->lock();
    active = obj->active;
    obj->unlock();
  } while (active);

  return 0;
}


void URGAutoCapture::lock(void) {
  SDL_LockMutex(mutex);
}


void URGAutoCapture::unlock(void) {
  SDL_UnlockMutex(mutex);
}


void URGAutoCapture::initCaptureBuffer(URGInterface::urgParams_t& params) {
  for (int i = 0; i < 2; ++i) {
    capture_buffer[i].length = new long [params.sense_steps];
    capture_buffer[i].captured = false;
    capture_buffer[i].raw_timestamp = 0;
    capture_buffer[i].capture_times = 0;

    capture_buffer[active_index].first = -1;
    capture_buffer[active_index].last = -1;
    capture_buffer[active_index].group = -1;
  }
  active_index = 0;
}


URGAutoCapture::URGAutoCapture(void)
  : mutex(SDL_CreateMutex()),
    active(true), next_first(-1), next_last(-1), next_group(-1),
    initialized(false), active_index(0), urg_params(NULL),
    auto_capture_times(0) {
  thread = SDL_CreateThread(captureThread, this);
}


URGAutoCapture::~URGAutoCapture(void) {

  // スレッドの終了を待つ
  lock();
  active = false;
  unlock();
  SDL_WaitThread(thread, NULL);

  // 資源の解放
  if (initialized) {
    for (int i = 0; i < 2; ++i) {
      delete [] capture_buffer[i].length;
    }
  }
  SDL_DestroyMutex(mutex);
}


int URGAutoCapture::capture(long length[],
                            int first_index, int last_index, int group,
                            URGInterface::urgParams_t& params,
                            unsigned long* raw_timestamp) {
  lock();
  next_first = first_index; next_last = last_index; next_group = group;
  urg_params = &params;
  if (!initialized) {
    initCaptureBuffer(params);
    initialized = true;
  }
  unlock();

  int timeout = params.cycle_msec << 2;
  bool captured = false;
  while (timeout > 0) {
    lock();
    captured =
      capture_buffer[active_index].captured
      & (capture_buffer[active_index].first == first_index)
      & (capture_buffer[active_index].last == last_index)
      & (capture_buffer[active_index].group == group);
    if (captured) {
      int ret_value = capture_buffer[active_index].ret_value;
      for (int i = 0; i < ret_value; ++i) {
        length[i] = capture_buffer[active_index].length[i];
      }
      if (raw_timestamp) {
        *raw_timestamp = capture_buffer[active_index].raw_timestamp;
      }
      auto_capture_times = capture_buffer[active_index].capture_times;
      unlock();
      return ret_value;
    }
    unlock();
    SDL_Delay(WaitMsec);
    timeout -= WaitMsec;
  }
  return ConnectionDevice::RecvTimeout;
}


int URGAutoCapture::getCaptureTimes(void) {
  return auto_capture_times;
}
#endif