runCtrl.cpp

/*
  走行制御クラス
  Satofumi KAMIMURA
  $Id$
*/

#include "runCtrl.h"
#include "tRunCtrl.h"
#include "serialDevice.h"
#include "tcpipDevice.h"
#include "fileUtils.h"
#include "packetHandleHost.h"
#include <stdio.h>


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

static const char* const B5conf = B5_CONF_FILE;


RunCtrl::RunCtrl(void)
  : con(NULL),
    node(&nodeResource), unique_id(0), crd_auto(true),
    local_offset(VXV::Position()),
    error_message("Connection device is not specified"),
    pre_module_msec(0), total_msec(0),
    tbl(&tblResource),
    FS(*new CoordinateCtrl()) {
  FS.setOwnCrdToObject(this);

  initNodeInfo(node);
  registerStructInfo(node, (unsigned char*)&tbl, RUN_CTRL_TARGET_ID);
  tbl->version = 0;
  initRunParams();
  registerCrdNortifyClient();
}


RunCtrl::~RunCtrl(void) {
  disconnect();
  delete &FS;
}


void RunCtrl::crdNortify(int state, const CoordinateCtrl* crd) {
  switch (state) {
  case Remove:
    // 保存してある移動コマンドの座標系に該当すれば、その移動コマンドを無効に
    for (std::list<runState_t>::iterator it = state_stack.begin();
         it != state_stack.end(); ++it) {
      if (it->command.crd == crd) {
        it->command.send_command_size = 0;
      }
    }
    break;

  case Update:
    // 現在の移動コマンド位置を再評価
    if (crd_auto) {
      runCommand_t& cmd = state_stack.front().command;
      sendUpdatedPosition(cmd.crd, cmd.position);
    }
    break;

  case Create:
    break;
  }
}


void RunCtrl::initRunParams(void) {
  runParams_t first_params;
  first_params.straight_ref_vel = StraightVel;
  first_params.straight_ref_acc = StraightAcc;
  first_params.rotate_ref_vel = RotateVel;
  first_params.rotate_ref_acc = RotateAcc;
  first_params.follow_r = FollowRadius;

  runCommand_t first_cmd;
  first_cmd.send_command_size = 0;
  first_cmd.position = VXV::Position();
  first_cmd.crd = VXV::GL;

  runState_t first_state;
  first_state.params = first_params;
  first_state.command = first_cmd;
  state_stack.push_front(first_state);
}


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


void RunCtrl::printHelp(void) {
  printf("Run Ctrl Library\n"
         "Options:\n"
         "--help                Display this information\n"
         "--version             Display Run Library version\n"
         "--run_port=[device]   Specify connection device\n"
         "--run_baudrate=[bps]  Specify connection baudrate\n"
         "\n\n");
}


void RunCtrl::printVersion(void) {
  printf("Run Ctrl Library " PACKAGE_STR_VERSION "\n");
}


bool RunCtrl::parseArgs(int* ret_value, int argc, char *argv[]) {

  char* device = NULL;
  long baudrate = Baudrate;
  bool simulator = false;
  bool help = false;
  bool version = false;

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

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

    } else if (!strcmp("--simulator", argv[i]) || !strcmp("-s", argv[i])) {
      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) {
    // バージョン情報の出力
    printVersion();
  }
  if (help) {
    // 使用例の表示
    printHelp();
  }
  if (simulator) {
    // シミュレータ接続
    *ret_value = connectSocket("localhost", SimulatorPort);
    return true;
  }
  if (device) {
    // デバイス接続
    disconnect();
    *ret_value = connect(device, baudrate);
    return true;
  }
  return false;
}


int RunCtrl::connect(int argc, char *argv[]) {

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


int RunCtrl::raw_connect(ConnectionDevice* conObj,
                         const char* device, long baudrate) {
  disconnect();
  con = conObj;
  int ret_value = con->connect(device, baudrate);
  error_message = con->what();
  if (ret_value >= 0) {
    ret_value = checkVersion();
  }
  return ret_value;
}


int RunCtrl::connect(const char* device, long baudrate) {
  return raw_connect(new SerialDevice(), device, baudrate);
}


int RunCtrl::connect(void) {

  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]);
    VXV::deleteArgs(args);
    if (ret) {
      return ret_value;
    }
  }
  return -1;
}


int RunCtrl::connectSocket(const char* host, long port) {
  con = new TcpipDevice();
  int ret_value = con->connect(host, port);
  error_message = con->what();
  if (ret_value >= 0) {
    ret_value = checkVersion();
  }
  return ret_value;
}


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


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


void RunCtrl::set_watchDogTimer(unsigned long msec) {
  sendWatchDogMsec(msec);
}


int RunCtrl::checkVersion(void) {

  long target_version = PACKAGE_NUM_VERSION -10;
  if (recvVersion(&target_version) < 0) {
    error_message = "Transmit fail: please check connected device: "
      + std::string(con->getDevice());
    return ConnectionDevice::FailCheckVersion;
  }
  if (target_version/10 != PACKAGE_NUM_VERSION/10) {
    fprintf(stderr,
            "warnning: RunCtrl version mismatch between PC(%d) and SH2(%ld)\n",
            PACKAGE_NUM_VERSION, target_version);
    exit(1);
  }
#if 1
  int ret_value = sendPositionInit();
  if (ret_value < 0) {
    error_message = "Transmit fail: in sendPositionInit()";
    return ret_value;
  }
#endif
  stop();

  set_watchDogTimer(0);
  initTicksInfo();

  // 現在自己位置で座標系を初期化
  adjustRunPosition(VXV::Position(0, 0, VXV::Direction()));

  return 0;
}


void RunCtrl::push_runState(void) {
  runState_t state = state_stack.front();
  state_stack.push_front(state);
}


void RunCtrl::pop_runState(void) {
  // 最低１つは残しておく
  if (state_stack.size() >= 2) {
    state_stack.pop_front();
  }
}


void RunCtrl::set_sendRetryTimes(int times) {
  set_runRetryTimes(times);
}


void RunCtrl::set_recvTimeout(int timeout) {
  set_runRecvTimeout(timeout);
}