urgSimulator.cpp

/*
  URG センサデバイスのシミュレートクラス
  Satofumi KAMIMURA
  $Id$
*/

#include "urgSimulator.h"
#include "urgCtrl.h"
#include "deleteObjects.h"
#include "monitorTask.h"
#include "urgIntersection.h"


URG_Simulator::URG_Simulator(void)
  : server(NULL), pre_registered(NULL), enable_timestamp(false) {
}


URG_Simulator::~URG_Simulator(void) {
  for_each(sim_urgs.begin(), sim_urgs.end(), DeleteObjects());
}


void URG_Simulator::init(void) {
  // サーバの起動
  server = new TcpipServer();
  server->activate(URGCtrl::SimulatorPort);
}


void URG_Simulator::recv(void) {
  // 接続
  TcpipDevice *con;
  if (pre_registered && (con = server->accept(0))) {
    ScanInfo* urg = new ScanInfo;
    urg->con = con;
    urg->obj = pre_registered;
    pre_registered = NULL;
    sim_urgs.push_back(urg);
  }

  // データの受信、応答
  for (std::list<ScanInfo*>::iterator it = sim_urgs.begin();
       it != sim_urgs.end(); ++it) {
    if (*it) {
      urgDeviceResponse(**it);
    }
  }
}


void URG_Simulator::exec1msec(unsigned long total_msec) {
  // 制御処理
  for (std::list<ScanInfo*>::iterator it = sim_urgs.begin();
       it != sim_urgs.end(); ++it) {
    if (*it) {
      urgDeviceSimulator(**it, total_msec);
    }
  }
}


URG_Simulator::ScanInfo::ScanInfo(void)
  : con(NULL), obj(NULL),
    pre_index(0), raw_timestamp(0),
    cmd_handled(true), now_timeAdjustMode(false) {
  for (int i = 0; i < 1024; ++i) {
    length[i] = 19;
  }
}


URG_Simulator::ScanInfo::~ScanInfo(void) {
  delete con;
  con = NULL;

  //sim_urgs.erase(this);
  // どこかでちゃんと削除すべき。切断の検出時かな？
}


void URG_Simulator::setURGObject(URGCtrl* urg_obj) {

  // !!! このあたり、同期とかしないと、複数 URG 使用時に非常にまずい
  pre_registered = urg_obj;
}


bool URG_Simulator::checkCmdLength(TcpipDevice* con, int require_size) {
  return (con->size() < require_size) ? true : false;
}


void URG_Simulator::urgDeviceResponse(URG_Simulator::ScanInfo& urg) {
  bool pre_cmd_handled = urg.cmd_handled;
  urg.cmd_handled = true;
  if (pre_cmd_handled || (urg.con->size() < 1)) {
    return;
  }

  char first_char;
  urg.con->copy(&first_char, 1);
  //fprintf(stderr, "first_char: %c\n", first_char);
  if (urg.now_timeAdjustMode && (first_char != 'T')) {
    // エラー応答
    char reply[5];
    sprintf(reply, "%c\rE\r\r", first_char);
    urg.con->send(reply, 5);
    return;
  }

  char buffer[8];
  switch (first_char) {
  case 'S':
    urg.con->recv(buffer, 8);
    break;

  case 'V':
    if (checkCmdLength(urg.con, 3)) {
      break;
    }
    replyVersionInfo(urg);
    break;

  case 'B':
    if (checkCmdLength(urg.con, 3)) {
      break;
    }
    urg.con->recv(buffer, 3);
    urg.con->send("BM\r00P\r\r", 8);
    break;

  case 'T':
    if (checkCmdLength(urg.con, 4)) {
      break;
    }
    replyAdjustTimestamp(urg);
    break;

    //case 'g':
  case 'G':
    if (checkCmdLength(urg.con, 13)) {
      break;
    }
    replyDataRequest(urg);
    break;
  }
}


void URG_Simulator::judgeSenseArea(URG_Simulator::ScanInfo& urg,
                                   bool isHandstand,
                                   int from_index, int to_index) {
  URGInterface::urgParams_t& params = urg.obj->getParameters();

  for (int i = from_index; i < to_index; ++i) {
    if (i > params.sense_steps) {
      continue;
    }
    double radian = urg.obj->index2rad(i);
    CoordinateCtrl::line_t line;
    line.position =
      VXV::Position(MonitorTask::getRunPosition()
                    + VXV::Position(0,0, VXV::Direction::rad(radian)));
    line.length = params.length_max;

    int dest_index = (!isHandstand) ? i : params.sense_steps - i;
    urg.length[dest_index] = getLengthToEnvironment(line, env_polygons);
  }
}


void URG_Simulator::urgDeviceSimulator(URG_Simulator::ScanInfo& urg,
                                       unsigned long total_msec) {
  URGInterface::urgParams_t& params = urg.obj->getParameters();
  int index =
    (urg.pre_index + (1000 / params.cycle_msec)) % params.cycle_step_max;
  int pre_index = urg.pre_index;
  urg.pre_index = index;

  // コマンド受付タイミング
  if (pre_index <= index) {
    if ((pre_index < params.sense_steps) && (params.sense_steps <= index)) {
      urg.cmd_handled = false;
    } else if (pre_index > index) {
      urg.cmd_handled = true;
    }
  }

  // タイムスタンプの確保
  if ((pre_index <= index) &&
      (pre_index < params.ticks_begin_step) &&
      (params.ticks_begin_step <= index)) {
    urg.raw_timestamp = total_msec & 0x00ffffff;
  }

  // 時刻印合わせモードの場合
  if (urg.now_timeAdjustMode) {
    urg.cmd_handled = false;
    urg.raw_timestamp = total_msec & 0x00ffffff;
  }

  // URG の取り付け情報を考慮
  if (urg.obj->isHandstand) {
    int tmp = index;
    index = params.cycle_step_max - pre_index;
    pre_index = params.cycle_step_max - tmp;
  }

  // 環境線分との交差判定処理
  if (pre_index > index) {
    judgeSenseArea(urg,urg.obj->isHandstand, pre_index, params.cycle_step_max);
    pre_index = 0;
  }
  judgeSenseArea(urg, urg.obj->isHandstand, pre_index, index);
}


void URG_Simulator::setURGType(const char* type) {
  if (!strcmp("URG-04LX", type)) {
    enable_timestamp = true;
  }
}


void URG_Simulator::replyVersionInfo(URG_Simulator::ScanInfo& urg) {
  char withTimestamp_reply[] =
    "VV\r"
    "VV\r"
    "00P\r"
    "VEND:Hokuyo Automatic Co.,Ltd.;[\r"
    "PROD:SOKUIKI Sensor TOP-URG UTM-X001S;E\r"
    "FIRM:k.1.28b(03/Sep./2007);M\r"
    "PROT:SCIP 2.0;N\r"
    "SERI:00704464;6\r"
    "\r";

  char normal_reply[] =
    "V\r"
    "0\r"
    "VEND:Hokuyo Automatic Co.,Ltd.\r"
    "PROD:SOKUIKI Sensor URG-X003\r"
    "FIRM:0.2.11d,05/05/18 with USB\r"
    "PROT:00003,(SCIP 1.0)\r"
    "SERI:H0500846\r"
    "\r";

  char buffer[3];
  urg.con->recv(buffer, 3);
  if (enable_timestamp) {
    urg.con->send(withTimestamp_reply, sizeof(withTimestamp_reply)-1);
  } else {
    urg.con->send(normal_reply,  sizeof(normal_reply)-1);
  }
}


void URG_Simulator::replyAdjustTimestamp(URG_Simulator::ScanInfo& urg) {
  char buffer[] = { "TMX\r00P\r\r23456\r\r" };
  char ticks_buffer[5];
  urg.con->recv(buffer, 4);
  long ticks;

  switch (buffer[2]) {
  case '0':
    urg.now_timeAdjustMode = true;
    urg.con->send(buffer, 9);
    break;

  case '1':
    ticks = static_cast<int>(urg.raw_timestamp);
    for (int i = 3; i >= 0; --i) {
      ticks_buffer[i] = decodeByte(ticks & 0x3f);
      ticks >>= 6;
    }
    ticks_buffer[5] = '\0';
    sprintf(buffer, "TM1\r00P\r%s;\r\r", ticks_buffer);
    urg.con->send(buffer, 16);
    break;

  case '2':
    urg.now_timeAdjustMode = false;
    urg.con->send(buffer, 9);
    break;
  }
}


char URG_Simulator::decodeByte(char ch) {
  return (((ch & 0x3f) | 0x40) - 0x10) & 0x7f;
}


void URG_Simulator::replyDataRequest(URG_Simulator::ScanInfo& urg) {

  enum { LF = '\n' };
  char buf[65];
  urg.con->recv(buf, 13);

  // 要求の取得
  char cmd = buf[0];
  char cmd2 = buf[1];
  int raw_group = atoi(&buf[10]);
  int group = (raw_group <= 0) ? 1 : raw_group;
  buf[10] = LF;
  int end_index = atoi(&buf[6]);
  buf[6] = LF;
  int begin_index = atoi(&buf[2]);

  // 保持しているデータを整形して返す
  sprintf(buf, "%c%c%04d%04d%02d%c",
          cmd, cmd2, begin_index, end_index, raw_group, LF);
  urg.con->send(buf, static_cast<int>(strlen(buf)));

  //if (cmd == 'g') {
  urg.con->send("00P\r", 4);

  unsigned long timestamp = urg.raw_timestamp;
  for (int i = 3; i >= 0; --i) {
    buf[i] = decodeByte((char)(timestamp & 0x3f));
    timestamp >>= 6;
  }
  buf[4] = ';';                 // 正規のチェックサムではない
  buf[5] = '\r';
  //sprintf(buf, "%06x%c", static_cast<int>(urg.raw_timestamp + 100 -40), LF);
  urg.con->send(buf, 6);
  //}

  // 測定データを返す
  char buffer[4096];            // !!! 適当。URG 専用だな
  int total_steps = (end_index - begin_index + 1) / abs(group);
  int data_index = 0;
  for (int i = 0; i < total_steps; ++i) {
    long length = urg.length[i];
    buffer[data_index++] = decodeByte((length >> 12) & 0x3f);
    buffer[data_index++] = decodeByte((length >> 6) & 0x3f);
    buffer[data_index++] = decodeByte(length & 0x3f);
  }

  int left_byte = data_index;
  int sended = 0;
  while (left_byte > 0) {
    int send_size = left_byte;
    if (send_size > 64) {
      send_size = 64;
    }
    urg.con->send(&buffer[sended], send_size);
    urg.con->send(";\r", 2);
    sended += send_size;
    left_byte -= send_size;
  }
  urg.con->send("\r", 1);

#if 0
  int total_steps = (end_index - begin_index + 1) / abs(group);
  int data_index = 0;
  int sensor_index = 0;
  for (int i = 0; i < total_steps; ++i) {
    if (i > 0 && i % 32 == 0) {
      buf[data_index++] = LF;
      urg.con->send(buf, data_index);
      data_index = 0;
    }

    int length = urg.length[sensor_index];
    for (int j = 0; j < 3; ++j) {
      buf[data_index++] = decodeByte((length >> 12) & 0x3f);
      length <<= 6;

      if (data_index != 0) {
        buf[data_index++] = LF;
      }
    }
    sensor_index += group;
  }
  buf[data_index++] = LF;
  urg.con->send(buf, data_index);
#endif
}


void URG_Simulator::setEnvironment(const std::vector<crd_polygon_t>& objs) {
  env_polygons = objs;
}