runCtrl.cpp

00001 /*
00002   走行制御クラス
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 
00007 #include "runCtrl.h"
00008 #include "tRunCtrl.h"
00009 #include "serialDevice.h"
00010 #include "tcpipDevice.h"
00011 #include "fileUtils.h"
00012 #include "packetHandleHost.h"
00013 #include <stdio.h>
00014 
00015 
00016 #ifndef B5_CONF_FILE
00017 #define B5_CONF_FILE "defaultargs"
00018 #endif
00019 #ifndef PACKAGE_STR_VERSION
00020 #define PACKAGE_STR_VERSION "2.1.0"
00021 #endif
00022 
00023 static const char* const B5conf = B5_CONF_FILE;
00024 
00025 
00026 RunCtrl::RunCtrl(void)
00027   : con(NULL),
00028     node(&nodeResource), unique_id(0), crd_auto(true),
00029     local_offset(VXV::Position()),
00030     error_message("Connection device is not specified"),
00031     pre_module_msec(0), total_msec(0),
00032     tbl(&tblResource),
00033     FS(*new CoordinateCtrl()) {
00034   FS.setOwnCrdToObject(this);
00035 
00036   initNodeInfo(node);
00037   registerStructInfo(node, (unsigned char*)&tbl, RUN_CTRL_TARGET_ID);
00038   tbl->version = 0;
00039   initRunParams();
00040   registerCrdNortifyClient();
00041 }
00042 
00043 
00044 RunCtrl::~RunCtrl(void) {
00045   disconnect();
00046   delete &FS;
00047 }
00048 
00049 
00050 void RunCtrl::crdNortify(int state, const CoordinateCtrl* crd) {
00051   switch (state) {
00052   case Remove:
00053     // 保存してある移動コマンドの座標系に該当すれば、その移動コマンドを無効に
00054     for (std::list<runState_t>::iterator it = state_stack.begin();
00055          it != state_stack.end(); ++it) {
00056       if (it->command.crd == crd) {
00057         it->command.send_command_size = 0;
00058       }
00059     }
00060     break;
00061 
00062   case Update:
00063     // 現在の移動コマンド位置を再評価
00064     if (crd_auto) {
00065       runCommand_t& cmd = state_stack.front().command;
00066       sendUpdatedPosition(cmd.crd, cmd.position);
00067     }
00068     break;
00069 
00070   case Create:
00071     break;
00072   }
00073 }
00074 
00075 
00076 void RunCtrl::initRunParams(void) {
00077   runParams_t first_params;
00078   first_params.straight_ref_vel = StraightVel;
00079   first_params.straight_ref_acc = StraightAcc;
00080   first_params.rotate_ref_vel = RotateVel;
00081   first_params.rotate_ref_acc = RotateAcc;
00082   first_params.follow_r = FollowRadius;
00083 
00084   runCommand_t first_cmd;
00085   first_cmd.send_command_size = 0;
00086   first_cmd.position = VXV::Position();
00087   first_cmd.crd = VXV::GL;
00088 
00089   runState_t first_state;
00090   first_state.params = first_params;
00091   first_state.command = first_cmd;
00092   state_stack.push_front(first_state);
00093 }
00094 
00095 
00096 const char* RunCtrl::what(void) {
00097   return error_message.c_str();
00098 }
00099 
00100 
00101 void RunCtrl::printHelp(void) {
00102   printf("Run Ctrl Library\n"
00103          "Options:\n"
00104          "--help                Display this information\n"
00105          "--version             Display Run Library version\n"
00106          "--run_port=[device]   Specify connection device\n"
00107          "--run_baudrate=[bps]  Specify connection baudrate\n"
00108          "\n\n");
00109 }
00110 
00111 
00112 void RunCtrl::printVersion(void) {
00113   printf("Run Ctrl Library " PACKAGE_STR_VERSION "\n");
00114 }
00115 
00116 
00117 bool RunCtrl::parseArgs(int* ret_value, int argc, char *argv[]) {
00118 
00119   char* device = NULL;
00120   long baudrate = Baudrate;
00121   bool simulator = false;
00122   bool help = false;
00123   bool version = false;
00124 
00125   for (int i = 0; i < argc; ++i) {
00126     if (!strncmp("--run_port=", argv[i], 11) && (strlen(argv[i]) > 11)) {
00127       device = &argv[i][11];
00128 
00129     } else if (!strncmp("--run_baudrate=", argv[i], 15) &&
00130                (strlen(argv[i]) > 15)) {
00131       baudrate = atoi(&argv[i][15]);
00132 
00133     } else if (!strcmp("--simulator", argv[i]) || !strcmp("-s", argv[i])) {
00134       simulator = true;
00135 
00136     } else if (!strcmp("--help", argv[i]) || !strcmp("-h", argv[i])) {
00137       help = true;
00138 
00139     } else if (!strcmp("--version", argv[i]) || !strcmp("-v", argv[i])) {
00140       version = true;
00141     }
00142   }
00143 
00144   if (version) {
00145     // バージョン情報の出力
00146     printVersion();
00147   }
00148   if (help) {
00149     // 使用例の表示
00150     printHelp();
00151   }
00152   if (simulator) {
00153     // シミュレータ接続
00154     *ret_value = connectSocket("localhost", SimulatorPort);
00155     return true;
00156   }
00157   if (device) {
00158     // デバイス接続
00159     disconnect();
00160     *ret_value = connect(device, baudrate);
00161     return true;
00162   }
00163   return false;
00164 }
00165 
00166 
00167 int RunCtrl::connect(int argc, char *argv[]) {
00168 
00169   int ret_value = -1;
00170   if (parseArgs(&ret_value, argc-1, &argv[1])) {
00171     return ret_value;
00172   }
00173   // デフォルト接続
00174   return connect();
00175 }
00176 
00177 
00178 int RunCtrl::raw_connect(ConnectionDevice* conObj,
00179                          const char* device, long baudrate) {
00180   disconnect();
00181   con = conObj;
00182   int ret_value = con->connect(device, baudrate);
00183   error_message = con->what();
00184   if (ret_value >= 0) {
00185     ret_value = checkVersion();
00186   }
00187   return ret_value;
00188 }
00189 
00190 
00191 int RunCtrl::connect(const char* device, long baudrate) {
00192   return raw_connect(new SerialDevice(), device, baudrate);
00193 }
00194 
00195 
00196 int RunCtrl::connect(void) {
00197 
00198   const char* home_str = getenv("HOME");
00199   std::string home_path = std::string((home_str ? home_str : ".")) + "/.vxv";
00200   const char* path[] = { ".", home_path.c_str(), NULL };
00201   std::string fname = VXV::searchFile(B5conf, path);
00202   if (!fname.empty()) {
00203     std::vector<char*> args;
00204     VXV::createArgs(args, fname.c_str());
00205     int ret_value = -1;
00206     bool ret = parseArgs(&ret_value, static_cast<int>(args.size()), &args[0]);
00207     VXV::deleteArgs(args);
00208     if (ret) {
00209       return ret_value;
00210     }
00211   }
00212   return -1;
00213 }
00214 
00215 
00216 int RunCtrl::connectSocket(const char* host, long port) {
00217   con = new TcpipDevice();
00218   int ret_value = con->connect(host, port);
00219   error_message = con->what();
00220   if (ret_value >= 0) {
00221     ret_value = checkVersion();
00222   }
00223   return ret_value;
00224 }
00225 
00226 
00227 void RunCtrl::disconnect(void) {
00228   if (con) {
00229     delete con;
00230     con = NULL;
00231   }
00232 }
00233 
00234 
00235 bool RunCtrl::isConnected(void) {
00236   return (con && con->isConnected()) ? true : false;
00237 }
00238 
00239 
00240 void RunCtrl::set_watchDogTimer(unsigned long msec) {
00241   sendWatchDogMsec(msec);
00242 }
00243 
00244 
00245 int RunCtrl::checkVersion(void) {
00246 
00247   long target_version = PACKAGE_NUM_VERSION -10;
00248   if (recvVersion(&target_version) < 0) {
00249     error_message = "Transmit fail: please check connected device: "
00250       + std::string(con->getDevice());
00251     return ConnectionDevice::FailCheckVersion;
00252   }
00253   if (target_version/10 != PACKAGE_NUM_VERSION/10) {
00254     fprintf(stderr,
00255             "warnning: RunCtrl version mismatch between PC(%d) and SH2(%ld)\n",
00256             PACKAGE_NUM_VERSION, target_version);
00257     exit(1);
00258   }
00259 #if 1
00260   int ret_value = sendPositionInit();
00261   if (ret_value < 0) {
00262     error_message = "Transmit fail: in sendPositionInit()";
00263     return ret_value;
00264   }
00265 #endif
00266   stop();
00267 
00268   set_watchDogTimer(0);
00269   initTicksInfo();
00270 
00271   // 現在自己位置で座標系を初期化
00272   adjustRunPosition(VXV::Position(0, 0, VXV::Direction()));
00273 
00274   return 0;
00275 }
00276 
00277 
00278 void RunCtrl::push_runState(void) {
00279   runState_t state = state_stack.front();
00280   state_stack.push_front(state);
00281 }
00282 
00283 
00284 void RunCtrl::pop_runState(void) {
00285   // 最低1つは残しておく
00286   if (state_stack.size() >= 2) {
00287     state_stack.pop_front();
00288   }
00289 }
00290 
00291 
00292 void RunCtrl::set_sendRetryTimes(int times) {
00293   set_runRetryTimes(times);
00294 }
00295 
00296 
00297 void RunCtrl::set_recvTimeout(int timeout) {
00298   set_runRecvTimeout(timeout);
00299 }
00300 

Generated on Mon Apr 13 22:52:04 2009 by  doxygen 1.5.7.1