urgCtrl.cpp

/*
  URG の制御クラス
  Satofumi KAMIMURA
  $Id$
*/

#include "urgCtrl.h"
#include "urgAutoCapture.h"
#include "fileUtils.h"
#include "ticksPosition.h"
#include <stdio.h>
#include <memory>


#ifndef B5_CONF_FILE
#define B5_CONF_FILE "defaultargs"
#endif
#ifndef PACKAGE_STR_VERSION
#define PACKAGE_STR_VERSION "2.0.0"
#endif

static const char* const B5conf = B5_CONF_FILE;
static const char* const FirstMessage = "Connection device is not specified";
static const char* const NotConnected = "URG is not connected";
bool URGCtrl::isVersionPrinted = false;
bool URGCtrl::isHelpPrinted = false;


URGCtrl::URGCtrl(void)
  : con(NULL), isHandstand(false), eachDataByte(2), enableOver4096(false),
    error_message(FirstMessage),
    ignore_beginTimeAdjust(false), pre_module_ticks(0), total_msec(0),
    to_simulator(false), enableTimestamp(false) {

  params.cycle_step_max = 1024;
  params.first_step = -1024 * 135 / 360;
  params.sense_steps = SenseSteps;
  params.rotate_direction = +1;
  params.cycle_msec = 100;
  params.length_min = 20;
  params.length_max = 4094;
  params.ticks_begin_step = 44;

  for (int i = 0; i < SenseSteps; ++i) {
    length[i] = -1;
  }
}


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


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


void URGCtrl::setHandstand(bool on) {
  isHandstand = on;
}


int URGCtrl::connect(const char* device, long baudrate, bool autoCapture) {
  disconnect();

  con = (autoCapture) ? new URGAutoCapture() : new URGManualCapture();
#if 0
  con = new UrgCtrlCapture;
  if (autoCapture) {
    // !!!
  }
#endif
  int ret_value = con->connect(device, baudrate);
  error_message = con->what();
  if (ret_value < 0) {
    return ret_value;
  }

  // バージョン情報の読み出し
  checkVersion();

  if (enableTimestamp) {
    initTicksInfo();
  }
  con->laser(true);
  return ret_value;
}


void URGCtrl::printVersion(void) {
  printf("URG Ctrl Library " PACKAGE_STR_VERSION "\n");
}


void URGCtrl::printHelp(void) {
  printf("\n"
         "----- URG Ctrl Library -----\n"
         "Options:\n"
         "-h,--help             Display this information\n"
         "-v,--version          Display URG Library version\n"
         "-s,--simulator        Connect to URG device simulator\n"
         "--urg_port=[device]   Specify connection device\n"
         "--urg_baudrate=[bps]  Specify connection baudrate\n"
         "--urg_handstand       Adjust URG settings\n"
         "\n");
}


bool URGCtrl::parseArgs(int* ret_value, int argc, char *argv[],
                        bool autoCapture) {
  char* device = NULL;
  long baudrate = DefaultBaudrate;
  bool help = false;
  bool version = false;

  for (int i = 0; i < argc; ++i) {
    if (!strncmp("--urg_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
      device = &argv[i][11];

    } else if (!strncmp("--urg_baudrate=", argv[i], 15) &&
               (strlen(argv[i]) > 15)) {
      baudrate = atoi(&argv[i][15]);

    } else if (!strcmp("--urg_handstand", argv[i])) {
      isHandstand = true;

    } else if (!strcmp("--simulator", argv[i]) || !strcmp("-s", argv[i])) {
      to_simulator = true;

    } else if (!strcmp("--help", argv[i]) || !strcmp("-h", argv[i])) {
      help = true;

    } else if (!strcmp("--version", argv[i]) || !strcmp("-v", argv[i])) {
      version = true;
    }
  }

  if (version && !isVersionPrinted) {
    // バージョン情報の出力
    printVersion();
    isVersionPrinted = true;
  }
  if (help && !isHelpPrinted) {
    // 使用例の表示
    printHelp();
    isHelpPrinted = true;
  }

  if (to_simulator) {
    return false;
  }
  if (device) {
    // デバイス接続
    *ret_value = connect(device, baudrate, autoCapture);
    return true;
  }
  return false;
}


int URGCtrl::connect(int argc, char *argv[], bool autoCapture) {
  disconnect();

  int ret_value = -1;
  if (parseArgs(&ret_value, argc-1, &argv[1], autoCapture)) {
    return ret_value;
  }
  // デフォルト接続
  return connect(autoCapture);
}


int URGCtrl::connect(bool autoCapture) {
  disconnect();
  const char* home_str = getenv("HOME");
  std::string home_path = std::string((home_str ? home_str : ".")) + "/.vxv";
  const char* path[] = { ".", home_path.c_str(), NULL };
  std::string fname = VXV::searchFile(B5conf, path);
  if (!fname.empty()) {
    std::vector<char*> args;
    VXV::createArgs(args, fname.c_str());
    int ret_value = -1;
    bool ret = parseArgs(&ret_value,
                         static_cast<int>(args.size()), &args[0], autoCapture);
    VXV::deleteArgs(args);
    if (!to_simulator && ret) {
      return ret_value;
    }
  }

  // --urg_handstand を反映させるため、B5conf を評価してから接続している
  // !!! --urg_handstand かどうかを記録し、再生時にはそれを用いるようにすべき
  if (to_simulator) {
    return connectSocket("localhost", (short)SimulatorPort, autoCapture);
  }

  return -1;
}


int URGCtrl::connectSocket(const char* host, short port, bool autoCapture) {
  disconnect();

  con = (autoCapture) ? new URGAutoCapture() : new URGManualCapture();
  int ret_value = con->connectSocket(host, port);
  error_message = con->what();

  // バージョン情報の読み出し
  checkVersion();

  if (enableTimestamp) {
    initTicksInfo();
  }
  return ret_value;
}


void URGCtrl::disconnect(void) {
  if (con) {
    delete con;
    con = NULL;
  }
  error_message = FirstMessage;
}


bool URGCtrl::isConnected(void) {
  return (con && con->isConnected()) ? true : false;
}


int URGCtrl::raw_capture(long length[],
                         int first_index, int last_index, int group,
                         unsigned long* raw_timestamp) {
  return con->capture(length, first_index, last_index, group,
                      params, raw_timestamp);
}


int URGCtrl::capture(int first_index, int last_index, int group) {
  if (!isConnected()) {
    throw URG_Exception(NotConnected);
  }

  int n;
  if (enableTimestamp) {
    unsigned long raw_ticks = 0;
    n = raw_capture(length, first_index, last_index, group, &raw_ticks);
    //fprintf(stderr, "UrgCtrl::n = %d\n", n);
    crd_ticks =
      getModuleTicks(updateTicksDiff(static_cast<unsigned short>(raw_ticks)));
    //fprintf(stderr, "crd_ticks: %d\n", crd_ticks);
  } else {
    n = raw_capture(length, first_index, last_index, group);
  }
  crd_position = getCrdPosition();

  if (isHandstand) {
    for (int i = (params.sense_steps) >> 1; i >= 0; --i) {
      long tmp = length[i];
      length[i] = length[params.sense_steps-1 - i];
      length[params.sense_steps-1 - i] = tmp;
    }
  }
  return n;
}


int URGCtrl::capture(int group) {
  return capture(0, params.sense_steps -1, group);
}


int URGCtrl::getCaptureTimes(void) {
  if (!isConnected()) {
    throw URG_Exception(NotConnected);
  }
  return con->getCaptureTimes();
}


void URGCtrl::convert(void) {
  convert(crd_position);
}


void URGCtrl::convert(const VXV::Position3D& position) {
  TicksPosition ticksPos;
  ticksPos.add(position, 0);
  convert(ticksPos);
}


int URGCtrl::getIndexTicks(int index) {

  int actual_index = (isHandstand) ? params.sense_steps - index : index;
  long ticks = static_cast<long>(1.0 * params.cycle_msec *
                                 (actual_index - params.ticks_begin_step)
                                 / params.cycle_step_max);
  return ticks;
}


void URGCtrl::convert(const VXV::TicksPositionInterface& ticksPos) {

  long pre_ticks = crd_ticks + getIndexTicks(0);
  long now_ticks = pre_ticks;
  VXV::Position3D urg_pos = ticksPos.getPosition(now_ticks);
  VXV::Position3D pre_urg_pos = urg_pos;
  VXV::Matrix4D convert = VXV::createConvertMatrix(urg_pos);
  crd_points.clear();
  for (int i = 0; i < params.sense_steps; ++i) {
    // 点毎の位置の計算
    now_ticks = crd_ticks + getIndexTicks(i);
    if (pre_ticks != now_ticks) {
      pre_ticks = now_ticks;
      urg_pos = ticksPos.getPosition(now_ticks);
      if (pre_urg_pos == urg_pos) {
        // !!! 何があったんだろうね、ここ

      } else {
        pre_urg_pos = urg_pos;
        //fprintf(stderr, "urg_pos: %d\n", urg_pos.zt.to_deg());
        convert = VXV::createConvertMatrix(urg_pos);
      }
    }
    // 点毎に座標変換
    long l = length[i];
    if ((l > params.length_min) && (l < params.length_max)) {
      VXV::Position3D point;
      point.x = static_cast<int>(length[i] * cos(index2rad(i)));
      point.y = static_cast<int>(length[i] * sin(index2rad(i)));
      VXV::convert(point, point, convert);
      crd_points.push_back(VXV::Grid3D(point.x, point.y, point.z));
    }
  }
}


void URGCtrl::set_recvTimeout(int timeout) {
  con->set_recvTimeout(timeout);
}