player_log_actarray: player_log_actarray: sweep_laser_scan

00001 #include <ncurses.h>
00002 #include <playerc.h>
00003 #include <string>
00004 #include <math.h>
00005 #include <stdio.h>
00006 #include <cstdlib>
00007 #include <getopt.h>
00008 #include <iostream>
00009 using namespace std;
00010 //#define M_PI        3.14159265358979323846
00011 // Convert radians to degrees
00012 #define RAD2DEG(r) (float)((r) * 180 / M_PI)
00013 // Convert degrees to radians
00014 #define DEG2RAD(d) (float)((d) * M_PI / 180)
00015 
00016 #define HOSTNAME "localhost"
00017 
00018 // what diference between angles is considered 0?
00019 #define EPS 0.01
00020 
00021 void home (playerc_client_t *client, playerc_actarray_t *device, int joint);
00022 void homeall (playerc_client_t *client, playerc_actarray_t *device);
00023 void sweep (playerc_client_t *client, playerc_actarray_t *device, 
00024             int actuator, float next_pos,  float speed);
00025 float go_to_pose (playerc_client_t *client, playerc_actarray_t *device, 
00026                   int actuator, float min_angle, float max_angle);
00027 void rotate (playerc_client_t *client, playerc_actarray_t *device, 
00028            int actuator, float angle, float speed);
00029 void print_data (playerc_actarray_t *device);
00030 void laser_power_on_off(playerc_client_t *client, playerc_laser_t *device_laser, bool sw);
00031 void usage_ ();
00032 //public struct for command line arguments
00033 static struct
00034 {
00035   char *program;
00036   std::string log_filename;     
00037   float start_angle;
00038   float end_angle;
00039   int interface;
00040   int rot_joint;
00041   //in rad/s
00042   float rot_speed;
00043   int loop;   
00044 } g_options;
00045 
00046 
00047 int main (int argc, char  **argv)
00048 {
00049 
00051   // Parse options
00053   g_options.program = argv[0];
00054  //set options to some impossible values
00055  g_options.log_filename = " ";
00056  g_options.start_angle = -1000;
00057  g_options.end_angle = -1000;
00058  g_options.interface = -1000;
00059  g_options.rot_joint = -1000;
00060  g_options.rot_speed = -1000;
00061  g_options.loop = 0;
00062  //use custom given log filename
00063  bool log_filename_from_time = false;
00064 
00065  while (1)
00066    {
00067      //for some reason specifying "optional_argument" results in empty valued option, 
00068      //thus all of them come with "required_argument" despite being optional  
00069      static struct option long_options[] = {
00070        {"help", no_argument, 0, 'h'},
00071        {"start_angle", required_argument, 0, 's'},
00072        {"end_angle", required_argument, 0, 'e'},
00073        {"interface", required_argument, 0, 'i'},
00074        {"rot_joint", required_argument, 0, 'j'},
00075        {"rot_speed", required_argument, 0, 'v'},
00076        {"log_filename", required_argument, 0, 'f'},
00077        {"loop", required_argument, 0, 'l'},
00078      };
00079      int option_index = 0;
00080      int c = getopt_long(argc, argv, "s:e:i:j:v:f:hl:", long_options, &option_index);
00081      if (c == -1) 
00082        {
00083          //usage_();
00084          break;
00085        }
00086      switch (c)
00087        {
00088        case 'h':
00089          usage_();
00090          break;
00091        case 's':
00092          g_options.start_angle = atof(optarg);
00093          break;
00094        case 'e':
00095          g_options.end_angle = atof(optarg);
00096          break;
00097        case 'i':
00098          g_options.interface = atoi(optarg);
00099          break;
00100        case 'j':
00101          g_options.rot_joint = atoi(optarg);
00102          break;
00103        case 'v':
00104          g_options.rot_speed = atof(optarg);
00105          break;
00106        case 'l':
00107          g_options.loop = atoi(optarg);
00108          break;
00109        case 'f':
00110           g_options.log_filename.assign(optarg);
00111           cerr << "log: " << g_options.log_filename << endl;
00112           break;
00113        }
00114    }
00115 
00116  if (g_options.interface == -1000)
00117    {
00118      cerr << "Error: You have to specify interface (-i)" << endl;
00119      exit(0);
00120    }
00121  if (g_options.start_angle  == -1000)
00122    {
00123      cerr << "Error: You have to specify start_angle (-s)" << endl;
00124      exit(0);
00125    }
00126  if (g_options.end_angle  == -1000)
00127    {
00128      cerr << "Error: You have to specify end_angle (-e)" << endl;
00129      exit(0);
00130    }
00131  if (g_options.rot_joint == -1000)
00132    {
00133      cerr << "Error: You have to specify rotational joint (-j)" << endl;
00134      exit(0);
00135    }
00136  if (g_options.rot_speed == -1000)
00137    {
00138    cerr << "Warning: Setting rot speed to 0.1 rad/s" << endl;
00139    g_options.rot_speed = 0.05;
00140    }
00141  if(g_options.log_filename == "")
00142    {
00143      cerr << "Warning: Using auto-generated log filenames!" << endl; 
00144      log_filename_from_time = true;
00145    }
00146 
00147 
00149  // main begins
00151  // exit(0);
00152 
00153  // Set the current actuator as the gripper (default)
00154   int delta_rot = 10;
00155   
00156   char logFileName[80];
00157   struct timeval t1;
00158   playerc_client_t *client;
00159   int t, i, j;
00160   void *rdevice;
00161   int port = 6665;
00162   playerc_actarray_t *device_actarray;
00163   playerc_laser_t *device_laser;
00164   playerc_log_t *device_log;
00165   client = playerc_client_create (NULL, HOSTNAME, port);
00166   playerc_client_connect (client);
00167 
00168   //return from go_to_pose
00169   float next_pos;
00170   
00171   device_actarray  = playerc_actarray_create (client, g_options.interface);
00172   device_laser  = playerc_laser_create (client, 0);
00173   device_log       = playerc_log_create      (client, 0);
00174 
00175   playerc_actarray_subscribe (device_actarray, PLAYER_OPEN_MODE);
00176   playerc_log_subscribe      (device_log, PLAYER_OPEN_MODE);
00177   playerc_laser_subscribe      (device_laser, PLAYER_OPEN_MODE);
00178 
00179   playerc_log_set_filename (device_log, g_options.log_filename.c_str());
00180   initscr ();
00181   refresh ();
00182   cbreak ();
00183   
00184   for (t = 0; t < 20; t++)
00185     rdevice = playerc_client_read (client);
00186 
00187   int current_actuator = device_actarray->actuators_count - 1;
00188   
00189   nodelay (stdscr, TRUE);
00190   keypad (stdscr, TRUE);
00191   
00192   printw ("Legend: 'p' - pause logging, 'r' - start/resume logging into from time generated log files, 'h' - home all cubes\n");
00193   printw ("        'b' - start/resume logging into cli passed log files\n");
00194   printw ("        's' - sweep a cube (selected using '1'-'%d')\n", device_actarray->actuators_count - 1);
00195   printw ("        'o' - home one cube, 'x' - exit\n"); refresh ();
00196   
00197   mvprintw (20, 0, "Logging : disabled\n"); refresh ();
00198   
00199   int key;
00200  
00201   if(g_options.loop == 1)
00202     {
00203       cerr << "loop   " << g_options.loop << endl;
00204       //exit(0);
00205       while (key != 'x')
00206         {
00207           laser_power_on_off(client, device_laser, false);
00208           rdevice = playerc_client_read (client);
00209           print_data (device_actarray);
00210           key = getch ();
00211           mvprintw (4, 0, "Keystroke: %d\n", key); refresh ();
00212           next_pos = go_to_pose(client, device_actarray, g_options.rot_joint, g_options.start_angle, g_options.end_angle);
00213           sweep (client, device_actarray, g_options.rot_joint, next_pos, g_options.rot_speed);
00214           if (next_pos == g_options.end_angle) 
00215             laser_power_on_off(client, device_laser, true);
00216           while (fabs(next_pos  -   RAD2DEG (device_actarray->actuators_data[g_options.rot_joint].position)) > EPS)
00217             {
00218               playerc_client_read (client);
00219               mvprintw (22, 0,  "Angle  %f\n", fabs(next_pos  -   RAD2DEG (device_actarray->actuators_data[g_options.rot_joint].position))); refresh();
00220               //sleep(1);
00221             }
00222         }
00223     }
00224  
00225   while (key != 'x')
00226   {
00227     rdevice = playerc_client_read (client);
00228 
00229     print_data (device_actarray);
00230     key = getch ();
00231     mvprintw (4, 0, "Keystroke: %d\n", key); refresh ();
00232      
00233     switch (key)
00234       {
00235       case '1':
00236         current_actuator = 0;
00237         break;
00238       case '2':
00239         current_actuator = 1;
00240         break;
00241       case '3':
00242         current_actuator = 2;
00243         break;
00244       case '4':
00245         current_actuator = 3;
00246         break;
00247       case '5':
00248         current_actuator = 4;
00249         break;
00250       case '6':
00251         current_actuator = 5;
00252         break;
00253       case '7':
00254         current_actuator = 6;
00255         break;
00256         
00257         // Left arrow
00258       case 260:
00259         delta_rot--;
00260         mvprintw (22, 0, "Delta : %d\n", delta_rot); refresh ();
00261         break;
00262         
00263         // Right arrow
00264       case 261:
00265         delta_rot++;
00266         mvprintw (22, 0, "Delta : %d\n", delta_rot); refresh ();
00267         break;
00268         
00269       case '-':
00270         // move - 10
00271         // rotate (client, device_actarray, current_actuator, -delta_rot, 0.1);
00272         mvprintw (21, 0, "Moving - with %d\n", delta_rot); refresh ();
00273         break;
00274       case '+':
00275         // move + 10
00276         //rotate (client, device_actarray, current_actuator, delta_rot, 0.1);
00277         mvprintw (21, 0, "Moving + with %d\n", delta_rot); refresh ();
00278         break;
00279         
00280       case 'p':
00281         // pause logging
00282         playerc_log_set_write_state (device_log, 0);
00283         mvprintw (20, 0, "Logging : disabled\n"); refresh ();
00284         break;
00285       case 'r':
00286         // resume logging
00287         if(log_filename_from_time)
00288           {
00289             gettimeofday(&t1, NULL);
00290             sprintf (logFileName, "%d.log", t1.tv_sec);
00291             playerc_log_set_filename (device_log, logFileName);
00292             playerc_log_set_write_state (device_log, 1);
00293             mvprintw (20, 0, "Logging : enabled\n"); refresh ();
00294           }
00295         else
00296           {
00297               mvprintw(20, 0, "This option not possible, use __\"b\"__ key to resume logging\n"); refresh();
00298           }
00299         break;
00300       case 'b':
00301         // resume logging, override custom log file
00302         // from g_options.log_filename;
00303         if(!log_filename_from_time)
00304           {
00305             //playerc_log_set_filename (device_log, g_options.log_filename.c_str());
00306             playerc_log_set_write_state (device_log, 1);
00307             mvprintw (20, 0, "Logging : enabled\n"); refresh ();
00308           }
00309         else
00310           {
00311             mvprintw(20, 0, "This option not possible, use __\"r\"__ key to resume logging\n"); refresh();
00312           }
00313         break;
00314       case 'h':
00315         // home
00316         homeall (client, device_actarray);
00317         break;
00318       case 'o':
00319         // home
00320         home (client, device_actarray, current_actuator);
00321         break;
00322       case 's':
00323         next_pos = go_to_pose(client, device_actarray, g_options.rot_joint, g_options.start_angle, g_options.end_angle);
00324         sweep (client, device_actarray, g_options.rot_joint, next_pos, g_options.rot_speed);
00325         break;
00326       }
00327     mvprintw (23, 0, "Current actuator: %d\n", (current_actuator + 1)); refresh ();
00328   }
00329  
00330   printw ("unsubscribing\n");refresh ();
00331   playerc_actarray_unsubscribe (device_actarray);
00332   playerc_actarray_destroy (device_actarray);
00333   
00334   playerc_log_unsubscribe (device_log);
00335   playerc_log_destroy (device_log);
00336 
00337   playerc_laser_unsubscribe (device_laser);
00338   playerc_laser_destroy (device_laser);
00339   
00340   endwin ();
00341   return 0;
00342 }
00343 
00344 // ---[ Home one cube
00345 void
00346     home (playerc_client_t *client, playerc_actarray_t *device, int joint)
00347 {
00348     int i;
00349 
00350     playerc_actarray_home_cmd (device, joint);
00351     do {
00352         playerc_client_read (client);
00353         print_data (device);
00354     }   
00355     while ((device->actuators_data[joint].state != PLAYER_ACTARRAY_ACTSTATE_IDLE) &&
00356            (device->actuators_data[joint].state != PLAYER_ACTARRAY_ACTSTATE_BRAKED));
00357 }
00358 
00359 // ---[ Home all cubes
00360 void
00361     homeall (playerc_client_t *client, playerc_actarray_t *device)
00362 {
00363     int i;
00364     playerc_actarray_power (device, 1);
00365     
00366     for (i = 6; i > -1; i--)
00367     {
00368         playerc_actarray_home_cmd (device, i);
00369         mvprintw (21, 0, "Homing %d\n", i); refresh ();
00370         do {
00371             playerc_client_read (client);
00372             print_data (device);
00373         }       
00374         while ((device->actuators_data[i].state != PLAYER_ACTARRAY_ACTSTATE_IDLE) &&
00375                (device->actuators_data[i].state != PLAYER_ACTARRAY_ACTSTATE_BRAKED));
00376     }
00377 }
00378 
00379 // ---[ Sweep a cube
00380 void
00381     sweep (playerc_client_t *client, playerc_actarray_t *device, 
00382             int actuator, float next_pos, float speed)
00383 {
00384   // Set the speed to move with
00385   playerc_actarray_speed_config (device, actuator, speed);
00386   
00387   mvprintw (21, 0, "Sweeping. Next position is %f \n", next_pos); refresh ();
00388   playerc_actarray_position_cmd (device, actuator, DEG2RAD (next_pos));
00389 
00390 }
00391 
00392 //find the start_angle
00393 float
00394     go_to_pose (playerc_client_t *client, playerc_actarray_t *device, 
00395             int actuator, float min_angle, float max_angle)
00396 {
00397   float next_pos;
00398   int key;
00399   
00400    // Find the current position
00401   float current_pos = RAD2DEG (device->actuators_data[actuator].position);
00402   
00403   // Go to either min_angle or max_angle depending which one is closer
00404   if (current_pos < min_angle)
00405     next_pos = min_angle;
00406   
00407   if (current_pos > max_angle)
00408     next_pos = max_angle;
00409   
00410   if (fabs (current_pos - min_angle) > fabs (current_pos - max_angle))
00411     next_pos = max_angle;
00412   else
00413     next_pos = min_angle;
00414   
00415   mvprintw (21, 0, "Init sweeping to: %f (dist to min: %f, dist to max:  %f) \n", next_pos, fabs (current_pos - min_angle), fabs (current_pos - max_angle)); refresh ();
00416   
00417   playerc_actarray_position_cmd (device, actuator, DEG2RAD (next_pos));
00418   // Loop until the actuator is idle-ing or break-ed.
00419   do {
00420     playerc_client_read (client);
00421     print_data (device);
00422     current_pos = RAD2DEG (device->actuators_data[actuator].position);
00423   }     
00424   while ( (fabs (current_pos - next_pos) > EPS) || (
00425                                                     (device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_IDLE) &&
00426                                                     (device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_BRAKED))
00427           );
00428     
00429   if (next_pos == max_angle)
00430     next_pos = min_angle;
00431   else
00432     next_pos = max_angle;
00433   
00434   return next_pos;
00435 }
00436 
00437 // ---[ Move a cube
00438 void
00439     rotate (playerc_client_t *client, playerc_actarray_t *device, 
00440             int actuator, float angle, float speed)
00441 {
00442     float next_pos;
00443 
00444     // Set the speed to move with
00445     //playerc_actarray_speed_config (device, actuator, speed);
00446     
00447     // Find the current position
00448     float current_pos = RAD2DEG (device->actuators_data[actuator].position);
00449     next_pos = current_pos + angle;
00450     
00451     playerc_actarray_position_cmd (device, actuator, DEG2RAD (next_pos));
00452     // Loop until the actuator is idle-ing or break-ed.
00453     do {
00454         playerc_client_read (client);
00455         print_data (device);
00456     }
00457     while ((device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_IDLE) &&
00458            (device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_BRAKED));
00459 }
00460 
00461 //print data to stdio
00462 void
00463     print_data (playerc_actarray_t *device)
00464 {
00465     int i;
00466     for (i = 0; i < device->actuators_count; i++)
00467     {
00468       mvprintw (10+i, 0, "X%d> pos, speed, accel, cur, state  : [%f, %f, %f, %f, %d]\n", (i+1), 
00469             RAD2DEG (device->actuators_data[i].position), 
00470             device->actuators_data[i].speed, 
00471             device->actuators_data[i].acceleration, 
00472             device->actuators_data[i].current, 
00473             device->actuators_data[i].state); refresh ();
00474     }
00475 }
00476 
00477 void 
00478 laser_power_on_off(playerc_client_t *client, playerc_laser_t *device_laser, bool sw)
00479 {
00480   //turn laser power on and off
00481   player_laser_power_config_t power_rq;
00482   power_rq.state = sw;
00483   void * dummy; 
00484   playerc_client_request(client, &device_laser->info, PLAYER_LASER_REQ_POWER,
00485                          reinterpret_cast <void *>(&power_rq), 
00486                          NULL);
00487  }
00488 
00489 //print CLI usage
00490 void usage_ ()
00491  {
00492    fprintf(stderr, "Usage: %s [options]\n", g_options.program);
00493    fprintf(stderr, "  Available options\n");
00494    fprintf(stderr, "    -i, --interface <0-right|1-left>\n");
00495    fprintf(stderr, "    -s, --start_angle\n");
00496    fprintf(stderr, "    -e, --end_angle\n");
00497    fprintf(stderr, "    -j, --rot_joint <0-5>\n");
00498    fprintf(stderr, "    -v, --rot_speed <rad/s>\n");
00499    fprintf(stderr, "    -f, --log_filename \n");
00500    fprintf(stderr, "    -h, --help Print this message and exit\n");
00501    exit(0);
00502  }
sweep_laser_scan_client.c
