ypspur-interpreter.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
00020 
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 #include <string.h>
00024 #include <signal.h>
00025 #include <setjmp.h>
00026 #include <unistd.h>
00027 #include <getopt.h>
00028 
00029 #ifdef HAVE_CONFIG_H
00030 #include <config.h>
00031 #endif  // HAVE_CONFIG_H
00032 
00033 #if HAVE_LIBREADLINE
00034 #include <readline/readline.h>
00035 #include <readline/history.h>
00036 #endif  // HAVE_LIBREADLINE
00037 
00038 #include <ypspur.h>
00039 #include <utility.h>
00040 
00041 typedef struct SPUR_COMMAND
00042 {
00043   enum
00044   {
00045     SPUR_NONE = -1,
00046     SPUR_LINE,
00047     SPUR_STOP_LINE,
00048     SPUR_CIRCLE,
00049     SPUR_SPIN,
00050     SPUR_GETPOS,
00051     SPUR_GETVEL,
00052     SPUR_SETPOS,
00053     SPUR_GETFORCE,
00054     SPUR_NEAR_POS,
00055     SPUR_NEAR_ANG,
00056     SPUR_OVER_LINE,
00057     SPUR_ADJUSTPOS,
00058     SPUR_SETVEL,
00059     SPUR_SETANGVEL,
00060     SPUR_SETACCEL,
00061     SPUR_SETANGACCEL,
00062     SPUR_INIT,
00063     SPUR_FREE,
00064     SPUR_OPENFREE,
00065     SPUR_STOP,
00066     SPUR_FREEZE,
00067     SPUR_UNFREEZE,
00068     SPUR_ISFREEZE,
00069     SPUR_GETAD,
00070     SPUR_SET_IO_DIR,
00071     SPUR_SET_IO_DATA,
00072     SPUR_GET_DEVICE_ERROR_STATE,
00073     SPUR_VEL,
00074     SPUR_WHEEL_VEL,
00075     SPUR_GET_WHEEL_VEL,
00076     SPUR_GET_WHEEL_ANG,
00077     SPUR_GET_WHEEL_TORQUE,
00078     SPUR_WHEEL_TORQUE,
00079     SPUR_GET_VREF,
00080     SPUR_GET_WHEEL_VREF,
00081     SPUR_ORIENT,
00082     SPUR_SET_WHEEL_VEL,
00083     SPUR_SET_WHEEL_ACCEL,
00084     SPUR_WHEEL_ANG,
00085     SPUR_JOINT_TORQUE,
00086     SPUR_JOINT_VEL,
00087     SPUR_JOINT_ANG,
00088     SPUR_JOINT_ANG_VEL,
00089     SPUR_SET_JOINT_ACCEL,
00090     SPUR_SET_JOINT_VEL,
00091     SPUR_GET_JOINT_VEL,
00092     SPUR_GET_JOINT_VREF,
00093     SPUR_GET_JOINT_ANG,
00094     SPUR_GET_JOINT_TORQUE,
00095     SPUR_SLEEP,
00096     EXIT,
00097     HELP,
00098     SPUR_COMMAND_MAX
00099   } id;
00100   char name[64];
00101   int reqarg;
00102   int narg;
00103   double arg[3];
00104 } SpurCommand;
00105 
00106 static const SpurCommand SPUR_COMMAND[SPUR_COMMAND_MAX] = {
00107   { SPUR_LINE, { "line" }, 3 },
00108   { SPUR_STOP_LINE, { "stop_line" }, 3 },
00109   { SPUR_CIRCLE, { "circle" }, 3 },
00110   { SPUR_SPIN, { "spin" }, 1 },
00111   { SPUR_GETPOS, { "get_pos" }, 0 },
00112   { SPUR_GETVEL, { "get_vel" }, 0 },
00113   { SPUR_GETFORCE, { "get_force" }, 0 },
00114   { SPUR_SETPOS, { "set_pos" }, 3 },
00115   { SPUR_NEAR_POS, { "near_pos" }, 3 },
00116   { SPUR_NEAR_ANG, { "near_ang" }, 2 },
00117   { SPUR_OVER_LINE, { "over_line" }, 3 },
00118   { SPUR_ADJUSTPOS, { "adjust_pos" }, 3 },
00119   { SPUR_SETVEL, { "set_vel" }, 1 },
00120   { SPUR_SETANGVEL, { "set_angvel" }, 1 },
00121   { SPUR_SETACCEL, { "set_accel" }, 1 },
00122   { SPUR_SETANGACCEL, { "set_angaccel" }, 1 },
00123   { SPUR_FREE, { "free" }, 0 },
00124   { SPUR_OPENFREE, { "openfree" }, 0 },
00125   { SPUR_STOP, { "stop" }, 0 },
00126   { SPUR_INIT, { "init" }, 0 },
00127   { SPUR_FREEZE, { "freeze" }, 0 },
00128   { SPUR_UNFREEZE, { "unfreeze" }, 0 },
00129   { SPUR_ISFREEZE, { "isfreeze" }, 0 },
00130   { SPUR_GETAD, { "get_ad_value" }, 1 },
00131   { SPUR_SET_IO_DIR, { "set_io_dir" }, 1 },
00132   { SPUR_SET_IO_DATA, { "set_io_data" }, 1 },
00133   { SPUR_GET_DEVICE_ERROR_STATE, { "get_device_error_state" }, 1 },
00134   { SPUR_VEL, { "vel" }, 2 },
00135   { SPUR_WHEEL_VEL, { "wheel_vel" }, 2 },
00136   { SPUR_GET_WHEEL_VEL, { "get_wheel_vel" }, 0 },
00137   { SPUR_GET_WHEEL_ANG, { "get_wheel_ang" }, 0 },
00138   { SPUR_GET_WHEEL_TORQUE, { "get_wheel_torque" }, 0 },
00139   { SPUR_WHEEL_TORQUE, { "wheel_torque" }, 2 },
00140   { SPUR_ORIENT, { "orient" }, 1 },
00141   { SPUR_GET_VREF, { "get_vref" }, 0 },
00142   { SPUR_GET_WHEEL_VREF, { "get_wheel_vref" }, 0 },
00143   { SPUR_SET_WHEEL_VEL, { "set_wheel_vel" }, 2 },
00144   { SPUR_SET_WHEEL_ACCEL, { "set_wheel_accel" }, 2 },
00145   { SPUR_WHEEL_ANG, { "wheel_ang" }, 2 },
00146   { SPUR_JOINT_TORQUE, { "joint_torque" }, 2 },
00147   { SPUR_JOINT_VEL, { "joint_vel" }, 2 },
00148   { SPUR_JOINT_ANG, { "joint_ang" }, 2 },
00149   { SPUR_JOINT_ANG_VEL, { "joint_ang_vel" }, 3 },
00150   { SPUR_SET_JOINT_ACCEL, { "set_joint_accel" }, 2 },
00151   { SPUR_SET_JOINT_VEL, { "set_joint_vel" }, 2 },
00152   { SPUR_GET_JOINT_VEL, { "get_joint_vel" }, 1 },
00153   { SPUR_GET_JOINT_VREF, { "get_joint_vref" }, 1 },
00154   { SPUR_GET_JOINT_ANG, { "get_joint_ang" }, 1 },
00155   { SPUR_GET_JOINT_TORQUE, { "get_joint_torque" }, 1 },
00156   { SPUR_SLEEP, { "sleep" }, 1 },
00157   { HELP, { "help" }, 0 },
00158   { EXIT, { "exit" }, 0 }
00159 };
00160 
00161 #if HAVE_SIGLONGJMP
00162 sigjmp_buf ctrlc_capture;
00163 #endif  // HAVE_SIGLONGJMP
00164 
00165 void ctrlc(int num)
00166 {
00167   printf("\n");
00168 #if HAVE_SIGLONGJMP
00169   siglongjmp(ctrlc_capture, 1);
00170 #endif  // HAVE_SIGLONGJMP
00171 }
00172 
00173 int proc_spur_cmd(char *line, int *coordinate)
00174 {
00175   int i;
00176   SpurCommand spur;
00177   int lcoordinate;
00178   char *argv;
00179   int ret;
00180   int ad;
00181   int error;
00182   enum
00183   {
00184     MODE_COMMAND,
00185     MODE_ARG
00186   } mode = MODE_COMMAND;
00187   char *toksave;
00188 
00189   spur.id = -1;
00190   lcoordinate = *coordinate;
00191 
00192   argv = strtok_r(line, " >\t\n\r", &toksave);
00193   if (!argv)
00194   {
00195     return 0;
00196   }
00197 
00198   while (argv)
00199   {
00200     switch (mode)
00201     {
00202       case MODE_COMMAND:
00203         for (i = 0; i < CS_MAX; i++)
00204         {
00205           if (strcmp(argv, YPSpur_CSName[i]) == 0)
00206           {
00207             lcoordinate = i;
00208             break;
00209           }
00210         }
00211         if (i != CS_MAX)
00212         {
00213           break;
00214         }
00215         for (i = 0; i < SPUR_COMMAND_MAX; i++)
00216         {
00217           if (strcmp(argv, SPUR_COMMAND[i].name) == 0)
00218           {
00219             spur = SPUR_COMMAND[i];
00220             break;
00221           }
00222         }
00223         mode = MODE_ARG;
00224         break;
00225       default:
00226         spur.arg[mode - MODE_ARG] = atof(argv);
00227         mode++;
00228         spur.narg = mode - MODE_ARG;
00229         if (mode > MODE_ARG + 3)
00230           break;
00231         break;
00232     }
00233     argv = strtok_r(NULL, " >\t\n\r", &toksave);
00234   }
00235 
00236   if (spur.id < 0)
00237   {
00238     *coordinate = lcoordinate;
00239     return 1;
00240   }
00241 
00242   if (spur.narg < spur.reqarg)
00243   {
00244     fprintf(stderr, "error: too few arguments to %s\n", spur.name);
00245     fflush(stderr);
00246     return 0;
00247   }
00248 
00249   switch (spur.id)
00250   {
00251     case SPUR_LINE:
00252       YPSpur_line(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00253       break;
00254     case SPUR_STOP_LINE:
00255       YPSpur_stop_line(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00256       break;
00257     case SPUR_CIRCLE:
00258       YPSpur_circle(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00259       break;
00260     case SPUR_ORIENT:
00261       YPSpur_orient(lcoordinate, spur.arg[0]);
00262       break;
00263     case SPUR_SPIN:
00264       YPSpur_spin(lcoordinate, spur.arg[0]);
00265       break;
00266     case SPUR_GETPOS:
00267       YPSpur_get_pos(lcoordinate, &spur.arg[0], &spur.arg[1], &spur.arg[2]);
00268       printf("%f %f %f\n", spur.arg[0], spur.arg[1], spur.arg[2]);
00269       break;
00270     case SPUR_GETVEL:
00271       YPSpur_get_vel(&spur.arg[0], &spur.arg[1]);
00272       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00273       break;
00274     case SPUR_GETFORCE:
00275       YPSpur_get_force(&spur.arg[0], &spur.arg[1]);
00276       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00277       break;
00278     case SPUR_GET_WHEEL_TORQUE:
00279       YP_get_wheel_torque(&spur.arg[0], &spur.arg[1]);
00280       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00281       break;
00282     case SPUR_GET_VREF:
00283       YP_get_vref(&spur.arg[0], &spur.arg[1]);
00284       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00285       break;
00286     case SPUR_GET_WHEEL_VREF:
00287       YP_get_wheel_vref(&spur.arg[0], &spur.arg[1]);
00288       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00289       break;
00290     case SPUR_SETPOS:
00291       YPSpur_set_pos(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00292       break;
00293     case SPUR_NEAR_POS:
00294       ret = YPSpur_near_pos(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00295       printf("%d\n", ret);
00296       break;
00297     case SPUR_NEAR_ANG:
00298       ret = YPSpur_near_ang(lcoordinate, spur.arg[0], spur.arg[1]);
00299       printf("%d\n", ret);
00300       break;
00301     case SPUR_OVER_LINE:
00302       ret = YPSpur_over_line(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00303       printf("%d\n", ret);
00304       break;
00305     case SPUR_ADJUSTPOS:
00306       YPSpur_adjust_pos(lcoordinate, spur.arg[0], spur.arg[1], spur.arg[2]);
00307       break;
00308     case SPUR_SETVEL:
00309       YPSpur_set_vel(spur.arg[0]);
00310       break;
00311     case SPUR_SETANGVEL:
00312       YPSpur_set_angvel(spur.arg[0]);
00313       break;
00314     case SPUR_SETACCEL:
00315       YPSpur_set_accel(spur.arg[0]);
00316       break;
00317     case SPUR_SETANGACCEL:
00318       YPSpur_set_angaccel(spur.arg[0]);
00319       break;
00320     case SPUR_STOP:
00321       YPSpur_stop();
00322       break;
00323     case SPUR_INIT:
00324       break;
00325     case SPUR_FREE:
00326       YPSpur_free();
00327       break;
00328     case SPUR_OPENFREE:
00329       YP_openfree();
00330       break;
00331     case SPUR_FREEZE:
00332       YPSpur_freeze();
00333       break;
00334     case SPUR_UNFREEZE:
00335       YPSpur_unfreeze();
00336       break;
00337     case SPUR_ISFREEZE:
00338       ret = YPSpur_isfreeze();
00339       printf("%d\n", ret);
00340       break;
00341     case SPUR_GETAD:
00342       ad = YP_get_ad_value((int)spur.arg[0]);
00343       printf("%d\n", ad);
00344       break;
00345     case SPUR_SET_IO_DIR:
00346       YP_set_io_dir((int)spur.arg[0]);
00347       break;
00348     case SPUR_SET_IO_DATA:
00349       YP_set_io_data((int)spur.arg[0]);
00350       break;
00351     case SPUR_GET_DEVICE_ERROR_STATE:
00352       YP_get_device_error_state((int)spur.arg[0], &error);
00353       printf("%d\n", error);
00354       break;
00355     case SPUR_VEL:
00356       YPSpur_vel(spur.arg[0], spur.arg[1]);
00357       break;
00358     case SPUR_WHEEL_VEL:
00359       YP_wheel_vel(spur.arg[0], spur.arg[1]);
00360       break;
00361     case SPUR_WHEEL_TORQUE:
00362       YP_wheel_torque(spur.arg[0], spur.arg[1]);
00363       break;
00364     case SPUR_GET_WHEEL_VEL:
00365       YP_get_wheel_vel(&spur.arg[0], &spur.arg[1]);
00366       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00367       break;
00368     case SPUR_GET_WHEEL_ANG:
00369       YP_get_wheel_ang(&spur.arg[0], &spur.arg[1]);
00370       printf("%f %f\n", spur.arg[0], spur.arg[1]);
00371       break;
00372     case SPUR_SET_WHEEL_VEL:
00373       YP_set_wheel_vel(spur.arg[0], spur.arg[1]);
00374       break;
00375     case SPUR_SET_WHEEL_ACCEL:
00376       YP_set_wheel_accel(spur.arg[0], spur.arg[1]);
00377       break;
00378     case SPUR_WHEEL_ANG:
00379       YP_wheel_ang(spur.arg[0], spur.arg[1]);
00380       break;
00381     case SPUR_JOINT_TORQUE:
00382       YP_joint_torque((int)spur.arg[0], spur.arg[1]);
00383       break;
00384     case SPUR_JOINT_VEL:
00385       YP_joint_vel((int)spur.arg[0], spur.arg[1]);
00386       break;
00387     case SPUR_JOINT_ANG:
00388       YP_joint_ang((int)spur.arg[0], spur.arg[1]);
00389       break;
00390     case SPUR_JOINT_ANG_VEL:
00391       YP_joint_ang_vel((int)spur.arg[0], spur.arg[1], spur.arg[2]);
00392       break;
00393     case SPUR_SET_JOINT_ACCEL:
00394       YP_set_joint_accel((int)spur.arg[0], spur.arg[1]);
00395       break;
00396     case SPUR_SET_JOINT_VEL:
00397       YP_set_joint_vel((int)spur.arg[0], spur.arg[1]);
00398       break;
00399     case SPUR_GET_JOINT_VEL:
00400       YP_get_joint_vel((int)spur.arg[0], &spur.arg[1]);
00401       printf("%f\n", spur.arg[1]);
00402       break;
00403     case SPUR_GET_JOINT_VREF:
00404       YP_get_joint_vref((int)spur.arg[0], &spur.arg[1]);
00405       printf("%f\n", spur.arg[1]);
00406       break;
00407     case SPUR_GET_JOINT_ANG:
00408       YP_get_joint_ang((int)spur.arg[0], &spur.arg[1]);
00409       printf("%f\n", spur.arg[1]);
00410       break;
00411     case SPUR_GET_JOINT_TORQUE:
00412       YP_get_joint_torque((int)spur.arg[0], &spur.arg[1]);
00413       printf("%f\n", spur.arg[1]);
00414       break;
00415     case SPUR_SLEEP:
00416       yp_usleep((int)(spur.arg[0] * 1000.0 * 1000.0));
00417       break;
00418     case HELP:
00419       printf("Usage:\n");
00420       printf("   > command arg1 arg2 arg3\n");
00421       printf("   > coordinate\n");
00422       printf("   > coordinate command arg1 arg2 arg3\n");
00423       printf("   > coordinate> command arg1 arg2 arg3\n");
00424       printf("Commands:\n");
00425       for (i = 0; i < SPUR_COMMAND_MAX; i++)
00426       {
00427         printf("   %s\n", SPUR_COMMAND[i].name);
00428       }
00429       printf("Coordinates:\n");
00430       for (i = 0; i < CS_MAX + 1; i++)
00431       {
00432         printf("   %s\n", YPSpur_CSName[i]);
00433       }
00434       break;
00435     case EXIT:
00436       return -1;
00437       break;
00438     default:
00439       printf("unknown command\n");
00440   }
00441   fflush(stdout);
00442 
00443   return 1;
00444 }
00445 
00446 int proc_spur(char *line, int *coordinate)
00447 {
00448   char *line_div;
00449   char *toksave;
00450   int ret;
00451 
00452   ret = 0;
00453   line_div = strtok_r(line, ";\n\r", &toksave);
00454   while (line_div)
00455   {
00456     ret = proc_spur_cmd(line_div, coordinate);
00457     if (ret < 0)
00458     {
00459       break;
00460     }
00461     line_div = strtok_r(NULL, ";\n\r", &toksave);
00462   }
00463   return ret;
00464 }
00465 
00466 void print_help(char *argv[])
00467 {
00468   fprintf(stderr, "USAGE: %s\n", argv[0]);
00469   fputs("\t-V | --set-vel      VALUE    : [m/s]   set max vel of SPUR to VALUE\n", stderr);
00470   fputs("\t-W | --set-angvel   VALUE    : [rad/s] set max angvel of SPUR to VALUE\n", stderr);
00471   fputs("\t-A | --set-accel    VALUE    : [m/ss]   set accel of SPUR to VALUE\n", stderr);
00472   fputs("\t-O | --set-angaccel VALUE    : [rad/ss] set angaccel of SPUR to VALUE\n", stderr);
00473   fputs("\t-c | --command     \"VALUE\"   : execute command VALUE\n", stderr);
00474   fputs("\t-q | --msq-id       VALUE    : set message-queue id\n", stderr);
00475   fputs("\t-s | --socket       IP:PORT  : use socket ipc\n", stderr);
00476   fputs("\t-h | --help                  : print this help\n", stderr);
00477 }
00478 
00479 int main(int argc, char *argv[])
00480 {
00481   int coordinate = CS_FS;
00482   char ip[64];
00483   int port = 0;
00484   int active = 1;
00485   int err = 0;
00486   double vel = 0;
00487   double angvel = 0;
00488   double accel = 0;
00489   double angaccel = 0;
00490   int set_vel = 0;
00491   int set_accel = 0;
00492   int set_angvel = 0;
00493   int set_angaccel = 0;
00494   int msqid = 0;
00495   struct option options[9] =
00496       {
00497         { "set-vel", 1, 0, 'V' },
00498         { "set-angvel", 1, 0, 'W' },
00499         { "set-accel", 1, 0, 'A' },
00500         { "set-angaccel", 1, 0, 'O' },
00501         { "command", 1, 0, 'c' },
00502         { "msq-id", 1, 0, 'q' },
00503         { "socket", 1, 0, 's' },
00504         { "help", 0, 0, 'h' },
00505         { 0, 0, 0, 0 }
00506       };
00507   int opt;
00508 
00509   hook_pre_global();
00510 
00511   while ((opt = getopt_long(argc, argv, "V:W:A:O:c:q:s:h", options, NULL)) != -1)
00512   {
00513     switch (opt)
00514     {
00515       case 'V':
00516         vel = atof(optarg);
00517         set_vel = 1;
00518         break;
00519       case 'W':
00520         angvel = atof(optarg);
00521         set_angvel = 1;
00522         break;
00523       case 'A':
00524         accel = atof(optarg);
00525         set_accel = 1;
00526         break;
00527       case 'O':
00528         angaccel = atof(optarg);
00529         set_angaccel = 1;
00530         break;
00531       case 'c':
00532         if (msqid == -1)
00533           YPSpur_init_socket(ip, port);
00534         else if (msqid == 0)
00535           YPSpur_init();
00536         else
00537           YPSpur_initex(msqid);
00538         proc_spur(optarg, &coordinate);
00539         return 1;
00540         break;
00541       case 'q':
00542         msqid = atoi(optarg);
00543         break;
00544       case 's':
00545         strncpy(ip, optarg, 64);
00546         {
00547           char *p;
00548           p = strchr(ip, ':');
00549           if (p == NULL)
00550           {
00551             fprintf(stderr, "USAGE: %s -s ip:port\n", argv[0]);
00552             return -1;
00553           }
00554           *p = 0;
00555           port = atoi(p + 1);
00556           msqid = -1;
00557         }
00558         break;
00559       case 'h':
00560         print_help(argv);
00561         return 1;
00562         break;
00563       default:
00564         break;
00565     }
00566   }
00567 
00568 #if HAVE_SIGLONGJMP
00569   signal(SIGINT, ctrlc);
00570 #endif  // HAVE_SIGLONGJMP
00571 
00572   if (msqid == -1)
00573     YPSpur_init_socket(ip, port);
00574   else if (msqid == 0)
00575     YPSpur_init();
00576   else
00577     YPSpur_initex(msqid);
00578 
00579   if (set_vel)
00580     YPSpur_set_vel(vel);
00581   if (set_angvel)
00582     YPSpur_set_angvel(angvel);
00583   if (set_accel)
00584     YPSpur_set_accel(accel);
00585   if (set_angaccel)
00586     YPSpur_set_angaccel(angaccel);
00587 
00588 #if HAVE_LIBREADLINE
00589   using_history();
00590   read_history(".spurip_history");
00591 #endif  // HAVE_LIBREADLINE
00592   while (active)
00593   {
00594     static char *line = NULL;
00595     static char *line_prev = NULL;
00596     char text[16];
00597 #if !HAVE_LIBREADLINE
00598 #if HAVE_GETLINE
00599     size_t len;
00600 #endif  // HAVE_GETLINE
00601 #endif  // !HAVE_LIBREADLINE
00602 #if HAVE_SIGLONGJMP
00603     if (sigsetjmp(ctrlc_capture, 1) != 0)
00604     {
00605 #if HAVE_LIBREADLINE
00606       write_history(".spurip_history");
00607 #endif  // HAVE_LIBREADLINE
00608     }
00609     else
00610 #endif  // HAVE_SIGLONGJMP
00611     {
00612       {
00613         // Dummy for error checking
00614         double x, y, th;
00615         YPSpur_get_pos(CS_GL, &x, &y, &th);
00616       }
00617       if (YP_get_error_state())
00618       {
00619         if (msqid == -1)
00620           YPSpur_init_socket(ip, port);
00621         else if (msqid == 0)
00622           YPSpur_init();
00623         else
00624           YPSpur_initex(msqid);
00625         if (set_vel)
00626           YPSpur_set_vel(vel);
00627         if (set_angvel)
00628           YPSpur_set_angvel(angvel);
00629         if (set_accel)
00630           YPSpur_set_accel(accel);
00631         if (set_angaccel)
00632           YPSpur_set_angaccel(angaccel);
00633         if (err == 0)
00634         {
00635           fprintf(stderr, "WARN: YPSpur-coordinator terminated.\n");
00636           fflush(stderr);
00637 #if HAVE_SIGLONGJMP
00638           signal(SIGINT, NULL);
00639 #endif  // HAVE_SIGLONGJMP
00640         }
00641         err = 1;
00642         yp_usleep(50000);
00643         continue;
00644       }
00645       else
00646       {
00647         if (err == 1)
00648         {
00649           fprintf(stderr, "INFO: YPSpur-coordinator started.\n");
00650           fflush(stderr);
00651 #if HAVE_SIGLONGJMP
00652           signal(SIGINT, ctrlc);
00653 #endif  // HAVE_SIGLONGJMP
00654         }
00655       }
00656       err = 0;
00657 
00658       sprintf(text, "%s> ", YPSpur_CSName[coordinate]);
00659 #if HAVE_LIBREADLINE
00660       line_prev = line;
00661       line = readline(text);
00662       if (!line)
00663       {
00664         // EOF
00665         break;
00666       }
00667       if (strlen(line) > 0)
00668       {
00669         if (line && line_prev)
00670         {
00671           if (strcmp(line, line_prev) != 0)
00672           {
00673             add_history(line);
00674           }
00675         }
00676         else
00677         {
00678           add_history(line);
00679         }
00680       }
00681 #else
00682       printf("%s", text);
00683       fflush(stdout);
00684       line = NULL;
00685 #if HAVE_GETLINE
00686       len = 0;
00687       getline(&line, &len, stdin);
00688       if (len == 0)
00689         continue;
00690 #else
00691       line = malloc(512);
00692       fgets(line, 512, stdin);
00693 #endif  // HAVE_GETLINE
00694       line_prev = line;
00695 #endif  // HAVE_LIBREADLINE
00696       if (proc_spur(line, &coordinate) < 0)
00697       {
00698         active = 0;
00699       }
00700 
00701       if (line_prev)
00702         free(line_prev);
00703     }
00704   }
00705   printf("\n");
00706 #if HAVE_LIBREADLINE
00707   write_history(".spurip_history");
00708 #endif  // HAVE_LIBREADLINE
00709 
00710   return 0;
00711 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19