urgManualCapture.cpp

/*
  URG の基本制御
  Satofumi KAMIMURA
  $Id$
*/

#include "urgManualCapture.h"
#include "serialDevice.h"
#include "tcpipDevice.h"
#include <stdio.h>

enum {
  LineLength = 64 + 16,
};


URGManualCapture::URGManualCapture(void)
  : con(NULL), error_message("Connection device is not specified"),
    capture_times(0), oneData_byte(OneDataByte), Timeout(DefaultTimeout),
    laser_on(false), remain_byte(0), enableTimestamp(false) {
}


URGManualCapture::~URGManualCapture(void) {
  disconnect();
}


const char* URGManualCapture::what(void) {
  return error_message.c_str();
}


int URGManualCapture::connect(const char* device, long baudrate) {
  disconnect();
  con = new SerialDevice();

  int ret_value = -1;
  long expected_baudrate[] = { 115200, 19200, 57600 };
  int n = sizeof(expected_baudrate) / sizeof(expected_baudrate[0]);
  char s_message[16];

  sprintf(s_message, "SS%06ld\r", baudrate);
  for (int try_times = 0; try_times < 2; ++try_times) {
    for (int i = 0; i < n; ++i) {

      ret_value = con->connect(device, expected_baudrate[i]);
      if (ret_value >= 0) {
        // Linux kernel 2.4 系での受信時に最初の 64[byte]が読めない問題に対処
        con->disconnect();
        ret_value = con->connect(device, expected_baudrate[i]);
      }
      error_message = con->what();
      if (ret_value < 0) {
        return ret_value;
      }
      con->flush();
      con->send(s_message, 9);

      char ch, pre_ch = '\0';
      while (con->recv(&ch, 1, Timeout) > 0) {
        if (isLF(ch) && isLF(pre_ch)) {
          error_message = "Success";
          con->setBaudrate(baudrate);
          return 0;
        }
        pre_ch = ch;
      }
    }
  }
  error_message = "Baudrate adjust fail";
  return ConnectionDevice::BaudrateAdjustFail;
}


int URGManualCapture::connectSocket(const char* host, short port) {

  disconnect();
  con = new TcpipDevice();
  int ret_value = con->connect(host, port);
  error_message = con->what();

  return ret_value;
}


void URGManualCapture::disconnect(void) {
  if (con) {
    delete con;
    con = NULL;
  }
}


bool URGManualCapture::isConnected(void) {
  if (!con) {
    return false;
  } else {
    if (con->isConnected()) {
      return true;
    } else {
      throw URGCapture_Exception("Connection is disconnected");
    }
  }
}


void URGManualCapture::setTimestampMode(bool withTimestamp) {
  enableTimestamp = withTimestamp;
}


bool URGManualCapture::isLF(char ch) {
  return (ch == '\r' || ch == '\n') ? true : false;
}


int URGManualCapture::adjustIndex(int index, int add) {

  int a = (index - (index / 65) + ((index >= 65) ? 1 : 0)) % 65;
  int b =
    (index - ((index + add) / 65) + ((index + add>= 65) ? 1 : 0) + add) % 65;
  if (a > b) {
      return index + add + 1;
  }
  return index + add;
}


unsigned char URGManualCapture::encode6bit(char ch) {
  return 0x3f & (ch - 0x30);
}


// !!! URGCtrl からのコピー
long URGManualCapture::decode(const char* data, int data_byte) {
  long value = 0;
  for (int i = 0; i < data_byte; ++i) {
    value <<= 6;
    value &= ~0x3f;
    value |= data[i] - 0x30;
  }
  return value;
}


int URGManualCapture::recvCaptureData(long* data, size_t max_size,
                                      unsigned long* timestamp,
                                      URGInterface::urgParams_t& params) {
#if 0
  if (data == NULL) {
    return 0;
  }
#endif

  recv_data.clear();

  // min までの領域を 19(計測有効範囲外)で埋める
  for (int i = params.first_step; i >= 0; --i) {
    recv_data.push_back(19);
  }

  char message_type = 'M';
  char buffer[LineLength];
  int line_length;
  unsigned long local_timestamp = 0;
  for (int i = 0; (line_length = readLine(buffer, Timeout)) >= 0; ++i) {

    //fprintf(stderr, "%02d: %s\n", i, buffer);

    // チェックサムの確認
    // !!!
    // !!! return false;

    if ((i >= 6) && (line_length == 0)) {
      // データ受信の完了
      int data_size = recv_data.size();
      size_t min_length =
        (static_cast<int>(max_size) < data_size) ? max_size : data_size;
      for (size_t i = 0; i < min_length; ++i) {
        data[i] = recv_data[i];
      }
      if (timestamp) {
        *timestamp = local_timestamp;
      }
      //fprintf(stderr, "ticks: %u\n", *timestamp);
      return min_length;

    } else if (i == 0) {
      // 送信メッセージの最初の文字のみでメッセージの判定を行う
      if ((buffer[0] != 'M') && (buffer[0] != 'G')) {
        error_message = "YY 01";
        //fprintf(stderr, "err 1\n");
        return -1;
      }
      message_type = buffer[0];

    } else if (! strncmp(buffer, "99b", 3)) {
      // "99b" を検出し、以降を「タイムスタンプ」「データ」とみなす
      i = 4;

    } else if ((i == 1) && (message_type == 'G')) {
      i = 4;

      // !!! 'G' コマンド時の残り回数は、ここで取り出せる
      if (0) {
        laser_on = false;
      }

    } else if (i == 2) {
      // 送信パケットに対する応答
      // !!!
      // 受信が終了したら、レーザーは停止する
      if (0) {
        laser_on = false;
      }

    } else if (i == 4) {
      // "99b" 固定
      if (strncmp(buffer, "99b", 3)) {
        error_message = "YY 02";
        return -1;
      }

    } else if (i == 5) {
      // タイムスタンプ
      local_timestamp = decode(buffer, 4);
      //fprintf(stderr, "ticks: %s, %d\n", buffer, local_timestamp);

    } else if (i >= 6) {
      // 取得データ
      if (line_length > (64 + 1)) {
        // !!! Top-URG のバグで 65 byte 以上のデータがくる問題に対処
        line_length = (64 + 1);
      }
      buffer[line_length -1] = '\0';
      int ret = addRecvData(buffer);
      if (ret < 0) {
        error_message = "YY 03";
        // !!! エラーメッセージの更新
        return ret;
      }
    }
  }
  return -1;
}


int URGManualCapture::readLine(char *buffer, int timeout) {
  if (con == NULL) {
    return 0;
  }

  int i;
  for (i = 0; i < LineLength -1; ++i) {
    char recv_ch;
    int n = con->recv(&recv_ch, 1, timeout);
    if (n <= 0) {
      if (i == 0) {
        return -1;              // タイムアウト
      }
      break;
    }
    if (isLF(recv_ch)) {
      break;
    }
    buffer[i] = recv_ch;
  }

  buffer[i] = '\0';
  return i;
}


int URGManualCapture::addRecvData(const char buffer[]) {

    const char* pre_p = buffer;
    const char* p = pre_p;

    if (remain_byte > 0) {
      memmove(&remain_data[remain_byte], buffer, oneData_byte - remain_byte);
      recv_data.push_back(decode(remain_data, oneData_byte));
      pre_p = &buffer[oneData_byte - remain_byte];
      p = pre_p;

      remain_byte = 0;
    }

    do {
      ++p;
      if ((p - pre_p) >= static_cast<int>(oneData_byte)) {
        recv_data.push_back(decode(pre_p, oneData_byte));
        pre_p = p;
      }
    } while (*p != '\0');
    remain_byte = p - pre_p;
    memmove(remain_data, pre_p, remain_byte);

    return 0;
  }


int URGManualCapture::recvScanData(long data[],
                                   int first, int last, int group,
                                   const char* send_str,
                                   URGInterface::urgParams_t& params,
                                   unsigned long* raw_ticks) {


  size_t max_size = last - first +1;
  ++capture_times;
  return recvCaptureData(data, max_size, raw_ticks, params);

#if 0
  char reply[13];
  char lf;

  if ((con->recv(reply, 13, DefaultTimeout) <= 0) ||
      strncmp(send_str, reply, 13)) {
    return -1;
  }
  if ((con->recv(reply, 4, Timeout) <= 0) || (reply[0] != '0')) {
    return -3;
  }

  //if (raw_ticks) {
  if (con->recv(reply, 6, Timeout) <= 0) {
    return HeaderError;
  }

  if (raw_ticks) {
    *raw_ticks = decode(reply, 4);
  }

  int num_data = last - first +1;
  num_data = (num_data / group) + ((num_data % group == 0) ? 0 : 1);
  int data_byte = oneData_byte * num_data;
  //int total = data_byte + ((data_byte >> 6) * 2 + 1);
  int total = data_byte;
  //fprintf(stderr, "total: %d\n", total);
  int filled = 0;

  while (total > filled) {
    int max = total - filled;
    if (max > 65) {
      max = 65;
    }
    int n = con->recv(&recv_buffer[filled], max, Timeout);
    if (n <= 0) {
      return RecvSizeError;
    }
    filled += n;
    if (n == 65) {
      filled -= 1;
    }
  }

  int lf_count = 0;
  while (true) {
    if (con->recv(&lf, 1, Timeout) <= 0) {
      return RecvSizeError;
    }
    if (!isLF(lf)) {
      lf_count = 0;
    }
    ++lf_count;
    if (lf_count >= 2) {
      break;
    }
  }

  //fprintf(stderr, "\n%s\n", recv_buffer);

  // 距離データを配列に格納
  for (int i = 0; i < first; ++i) {
    data[i] = -1;
  }
  int index = first;
  for (int i = 0; (index <= last) && (i < num_data); ++i) {
    int num = i * oneData_byte;
#if 0
    int actual_index = num + (int)(num / 65);
    if ((i > 0) && ((i % 65) == 0) &&
        !isLF(recv_buffer[actual_index -1])) {
      return MissmatchLF;
    }
#endif
    int actual_index = num;
    for (int j = 0; (index <= last) && (j < group); ++j) {
      data[index] =
        (encode6bit(recv_buffer[adjustIndex(actual_index, 0)]) << 6)
        | encode6bit(recv_buffer[adjustIndex(actual_index, 1)]);
      if (oneData_byte == 3) {
        data[index] = (data[index] << 6)
          | encode6bit(recv_buffer[adjustIndex(actual_index, 2)]);
      }
      ++index;
      //fprintf(stderr, "%d: %c%c%c, ", actual_index, recv_buffer[adjustIndex(actual_index, 0)], recv_buffer[adjustIndex(actual_index, 1)], recv_buffer[adjustIndex(actual_index, 2)]);
    }
  }
  for (int i = last +1; i < params.sense_steps; ++i) {
    data[i] = -1;
  }
  ++capture_times;

#if 0
  for (int i = 0; i < params.sense_steps; ++i) {
    fprintf(stderr, "%d:%d\n", i, data[i]);
  }
  fprintf(stderr,"\n");
#endif

  return params.sense_steps;
#endif
}


int URGManualCapture::capture(long length[],
                              int first_index, int last_index, int group,
                              URGInterface::urgParams_t& params,
                              unsigned long* raw_timestamp) {
  // 渡された値を修正
  if (first_index > last_index) {
    int tmp = first_index; first_index = last_index; last_index = tmp;
  }
  if (first_index < 0) {
    first_index = 0;
  }
  if (last_index > params.sense_steps -1) {
    last_index = params.sense_steps -1;
  }
  if (group <= 0) {
    group = 1;
  } else if (group > 99) {
    group = 99;
  }

  // 取得コマンドの送信
  char send_str[13];
  sprintf(send_str, "GD%04d%04d%02d\n", first_index, last_index, group);
#if 0
  if (raw_timestamp) {
    send_str[0] = 'g';
  }
#endif
  con->flush();
  con->send(send_str, 13);

  // 受信データの解析
  int ret = recvScanData(length, first_index, last_index, group,
                         send_str, params, raw_timestamp);
  if (ret < 0) {
    error_message = "capture fail.";
  }
  return ret;
}


int URGManualCapture::getCaptureTimes(void) {
  return capture_times;
}


int URGManualCapture::send(const char* data, int size) {
  if (!con) {
    return ConnectionDevice::NotConnected;
  }
  return con->send(data, size);
}


int URGManualCapture::recv(char* data, int maxsize, long timeout) {
  if (!con) {
    return ConnectionDevice::NotConnected;
  }
  return con->recv(data, maxsize, timeout);
}


int URGManualCapture::recv_line(char* data, int maxsize, long timeout) {
  if (!con) {
    return ConnectionDevice::NotConnected;
  }

  // 改行までを読み出して data に格納する
  int filled = 0;
  while (con->recv(&data[filled], ((maxsize - filled > 0) ? 1 : 0),
                   timeout) > 0) {
    if (isLF(data[filled++])) {
      --filled;
      data[filled] = '\0';
      //fprintf(stderr, "%s\n", data);
      return filled;
    }
  }
  return filled;
}


void URGManualCapture::set_recvTimeout(int timeout) {
  Timeout = (timeout < 0) ? DefaultTimeout : timeout;
}


int URGManualCapture::laser(bool on) {
  //char l_message[7] = "L0\r";
  char l_message[9] = "BM\r";
  //l_message[1] = (on) ? '1' : '0';
  con->flush();
  con->send(l_message, 3);

  char ch, pre_ch = '\0';
  while (con->recv(&ch, 1, Timeout + 200) > 0) {
    //fprintf(stderr, "%c", ch);
    if (isLF(ch) && isLF(pre_ch)) {
      return 0;
    }
    pre_ch = ch;
  }

  //con->recv(l_message, 8, DefaultTimeout);
  //con->recv(l_message, 8, Timeout);

  // !!! とりあえず、適当で
  return 0; //(l_message[3] == '0') ? 0 : -1;
  return -1;
}