Fawkes API  Fawkes Development Version
ffptu.cpp
00001 
00002 /***************************************************************************
00003  *  ffptu.cpp - Control PTU via keyboard
00004  *
00005  *  Created: Thu Oct 06 16:28:16 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "../robotis/rx28.h"
00024 
00025 #include <core/exceptions/system.h>
00026 #include <utils/system/argparser.h>
00027 #include <logging/console.h>
00028 #include <utils/system/getkey.h>
00029 #include <utils/time/time.h>
00030 #include <blackboard/remote.h>
00031 #include <interface/interface_info.h>
00032 
00033 #include <cstdlib>
00034 #include <cstdio>
00035 #include <cstring>
00036 #include <unistd.h>
00037 
00038 #include <interfaces/PanTiltInterface.h>
00039 
00040 using namespace fawkes;
00041 
00042 void
00043 print_usage(const char *program_name)
00044 {
00045   printf("Usage: %s [-h] [-r host[:port]] <command>\n"
00046          " Options:\n"
00047          " -h              This help message\n"
00048          " -r host[:port]  Remote host (and optionally port) to connect to\n"
00049          " -p <ptu>        PTU, interface must have id 'PanTilt <ptu>'\n"
00050          " -l              List available PTUs\n"
00051          " -i              Invert tilt control buttons\n\n"
00052          " Commands:\n"
00053          "move [pan P] [tilt T]\n"
00054          "  Move PTU, if either pan or tilt are supplied, send the\n"
00055          "  command and wait for angle to be reached. Otherwise enter\n"
00056          "  interactive mode\n"
00057          "rx28-set-id <old-id> <new-id>\n"
00058          "  Send message to set a new servo ID.\n"
00059          "  WARNING: use this with only a single servo connected!\n"
00060          "rx28-discover\n"
00061          "  Discover and print servos on the BUS.\n",
00062          program_name);
00063 }
00064 
00065 
00066 /** Remote control PTUs via keyboard.
00067  * @author Tim Niemueller
00068  */
00069 class PTUJoystickControl
00070 {
00071  public:
00072   /** Constructor.
00073    * @param argc number of arguments in argv
00074    * @param argv array of parameters passed into the program
00075    */
00076   PTUJoystickControl(int argc, char **argv)
00077     : __argp(argc, argv, "hr:p:li")
00078   {
00079     __rx28 = NULL;
00080     __bb = NULL;
00081     __ptu_if = NULL;
00082     __resolution = 0.1f;
00083 
00084     if ( __argp.has_arg("h") ) {
00085       print_usage(argv[0]);
00086       exit(0);
00087     }
00088   }
00089 
00090   /** Destructor. */
00091   ~PTUJoystickControl()
00092   {
00093     if (__bb) {
00094       __bb->close(__ptu_if);
00095       delete __bb;
00096     }
00097     delete __rx28;
00098   }
00099 
00100 
00101   /** Initialize BB connection. */
00102   void init_bb()
00103   {
00104     char *host = (char *)"localhost";
00105     unsigned short int port = 1910;
00106     bool free_host = __argp.parse_hostport("r", &host, &port);
00107 
00108     __bb = new RemoteBlackBoard(host, port);
00109     if (free_host)  free(host);
00110 
00111     if (__argp.has_arg("l")) {
00112       InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *");
00113       if (l->empty()) {
00114         printf("No interfaces found");
00115       }
00116       for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
00117         std::string id = i->id();
00118         id = id.substr(std::string("PanTilt ").length());
00119 
00120         printf("PTU: %s   Writer: %s\n", id.c_str(),
00121                i->has_writer() ? "Yes" : "No");
00122       }
00123       delete l;
00124       return;
00125     }
00126 
00127     if (__argp.has_arg("p")) {
00128       std::string iface_id = std::string("PanTilt ") + __argp.arg("p");
00129       __ptu_if = __bb->open_for_reading<PanTiltInterface>(iface_id.c_str());
00130     } else {
00131       InterfaceInfoList *l = __bb->list("PanTiltInterface", "PanTilt *");
00132       if (l->empty()) {
00133         throw Exception("No PanTilt interface opened on remote!");
00134       }
00135       for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
00136         if (i->has_writer()) {
00137           __ptu_if = __bb->open_for_reading<PanTiltInterface>(i->id());
00138         } else {
00139           printf("Interface %s has no writer, ignoring\n", i->id());
00140         }
00141       }
00142       delete l;
00143     }
00144 
00145     if (!__ptu_if) {
00146       throw Exception("No suitable PanTiltInterface found");
00147     } 
00148 
00149   }
00150 
00151   /** Initialize Robotis RX28 raw servo access. */
00152   void init_rx28()
00153   {
00154     __rx28 = new RobotisRX28("/dev/ttyUSB0");
00155     RobotisRX28::DeviceList devl = __rx28->discover();
00156 
00157     if (devl.empty()) {
00158       throw Exception("No devices found\n");
00159       
00160     }
00161 
00162   }
00163 
00164 
00165   /** Run control loop. */
00166   void run()
00167   {
00168     if (__argp.num_items() == 0) {
00169       interactive_move();
00170     } else if (strcmp(__argp.items()[0], "move") == 0) {
00171       if (__argp.num_items() == 1) {
00172         interactive_move();
00173       } else {
00174         exec_move();
00175       }
00176     } else if (strcmp(__argp.items()[0], "rx28-set-id") == 0) {
00177       rx28_set_id();
00178 
00179     } else if (strcmp(__argp.items()[0], "rx28-discover") == 0) {
00180       rx28_discover();
00181 
00182     } else {
00183       printf("Unknown command '%s'\n", __argp.items()[0]);
00184     }
00185   }
00186 
00187  private:
00188 
00189   void interactive_move()
00190   {
00191     init_bb();
00192 
00193     Time last, now;
00194 
00195     char tilt_up = 65;
00196     char tilt_down = 66;
00197     if (__argp.has_arg("i")) std::swap(tilt_up, tilt_down);
00198 
00199     float pan, tilt, new_pan, new_tilt;
00200     float speed = 0.0, new_speed = 0.5;
00201 
00202     __ptu_if->read();
00203     pan  = new_pan  = __ptu_if->pan();
00204     tilt = new_tilt = __ptu_if->tilt();
00205 
00206     last.stamp();
00207     char key = 0;
00208     int wait_time = 5;
00209     while (key != 'q') {
00210       key = getkey(wait_time);
00211       //printf("Key: %u = %u\n", key, key);
00212       if (key != 0) {
00213         now.stamp();
00214         if ( (now - &last) < 0.5) {
00215           wait_time = 1;
00216         }
00217         last.stamp();
00218       }
00219       if (key == 0) {
00220         wait_time = 5;
00221         
00222       } else if (key == 27) {
00223         key = getkey();
00224         if (key == 0) {
00225           // Escape key
00226           new_pan = pan;
00227           new_tilt = tilt;
00228         } else {
00229           if (key != 91) continue;
00230 
00231           key = getkey();
00232           if (key == 0) continue;
00233 
00234           if (key == tilt_up) {
00235             new_tilt = std::min(tilt + __resolution, __ptu_if->max_tilt());
00236           } else if (key == tilt_down) {
00237             new_tilt = std::max(tilt - __resolution, __ptu_if->min_tilt());
00238           } else if (key == 67) {
00239             new_pan = std::max(pan - __resolution, __ptu_if->min_pan());
00240           } else if (key == 68) {
00241             new_pan = std::min(pan + __resolution, __ptu_if->max_pan());
00242           } else continue;
00243 
00244         }
00245       } else if (key == '0') {
00246         new_pan = new_tilt = 0.f;
00247       } else if (key == '9') {
00248         new_pan = 0;
00249         new_tilt = M_PI / 2.;
00250       } else if (key == 'r') {
00251         __resolution = 0.1f;
00252       } else if (key == 'R') {
00253         __resolution = 0.01f;
00254       } else if (key == '+') {
00255         new_speed = std::min(speed + 0.1, 1.0);
00256       } else if (key == '-') {
00257         new_speed = std::max(speed - 0.1, 0.0);
00258       }
00259 
00260       if (speed != new_speed) {
00261         speed = new_speed;
00262         float pan_vel  = speed * __ptu_if->max_pan_velocity();
00263         float tilt_vel = speed * __ptu_if->max_tilt_velocity();
00264 
00265         printf("Setting velocity %f/%f (max %f/%f)\n", pan_vel, tilt_vel,
00266                __ptu_if->max_pan_velocity(), __ptu_if->max_tilt_velocity());
00267 
00268         PanTiltInterface::SetVelocityMessage *svm =
00269           new PanTiltInterface::SetVelocityMessage(pan_vel, tilt_vel);
00270         __ptu_if->msgq_enqueue(svm);
00271       }
00272 
00273       if ((pan != new_pan) || (tilt != new_tilt)) {
00274         pan = new_pan;
00275         tilt = new_tilt;
00276 
00277         printf("Goto %f/%f\n", pan, tilt);
00278 
00279         PanTiltInterface::GotoMessage *gm =
00280           new PanTiltInterface::GotoMessage(pan, tilt);
00281         __ptu_if->msgq_enqueue(gm);        
00282       }
00283     }
00284   }
00285 
00286 
00287   void exec_move()
00288   {
00289     init_bb();
00290 
00291     float pan, tilt, new_pan, new_tilt;
00292 
00293     __ptu_if->read();
00294     pan  = new_pan  = __ptu_if->pan();
00295     tilt = new_tilt = __ptu_if->tilt();
00296 
00297     const std::vector< const char * > &items = __argp.items();
00298     for (unsigned int i = 1; i < items.size(); ++i) {
00299       if (strcmp(items[i], "pan") == 0) {
00300         if (items.size() > i+1) {
00301           new_pan = __argp.parse_item_float(++i);
00302         } else {
00303           printf("No pan value supplied, aborting.\n");
00304           return;
00305         }
00306       } else if (strcmp(items[i], "tilt") == 0) {
00307         if (items.size() > i+1) {
00308           new_tilt = __argp.parse_item_float(++i);
00309         } else {
00310           printf("No tilt value supplied, aborting.\n");
00311           return;
00312         }
00313       } else {
00314         printf("Unknown parameter '%s', aborting.\n", items[i]);
00315         return;
00316       }
00317     }
00318 
00319     if ((pan != new_pan) || (tilt != new_tilt)) {
00320       printf("Goto pan %f and tilt %f\n", new_pan, new_tilt);
00321 
00322       PanTiltInterface::SetVelocityMessage *svm =
00323         new PanTiltInterface::SetVelocityMessage(__ptu_if->max_pan_velocity() / 2.,
00324                                                  __ptu_if->max_tilt_velocity() / 2.);
00325       __ptu_if->msgq_enqueue(svm);
00326 
00327       PanTiltInterface::GotoMessage *gm =
00328         new PanTiltInterface::GotoMessage(new_pan, new_tilt);
00329       __ptu_if->msgq_enqueue(gm);
00330       usleep(5e5);
00331     }
00332   }
00333 
00334 
00335   void rx28_set_id()
00336   {
00337     init_rx28();
00338 
00339     int old_id = __argp.parse_item_int(1);
00340     int new_id = __argp.parse_item_int(2);
00341 
00342     printf("Servo IDs *before* setting the ID:\n");
00343     RobotisRX28::DeviceList devl = __rx28->discover();
00344     for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
00345       printf("  %d\n", *i);
00346     }
00347 
00348     if (old_id < 0 || old_id >= RobotisRX28::BROADCAST_ID) {
00349       printf("Invalid old ID %i, must be in range [0..%u]\n",
00350              old_id, RobotisRX28::BROADCAST_ID);
00351       return;
00352     }
00353 
00354     if (new_id < 0 || new_id >= RobotisRX28::BROADCAST_ID) {
00355       printf("Invalid new ID %i, must be in range [0..%u]\n",
00356              new_id, RobotisRX28::BROADCAST_ID);
00357       return;
00358     }
00359 
00360     __rx28->set_id(old_id, new_id);
00361 
00362     printf("Servo IDs *after* setting the ID:\n");
00363     devl = __rx28->discover();
00364     for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
00365       printf("  %d\n", *i);
00366     }
00367   }
00368 
00369   void rx28_discover()
00370   {
00371     init_rx28();
00372 
00373     printf("Servo IDs on the bus:\n");
00374     RobotisRX28::DeviceList devl = __rx28->discover();
00375     for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
00376       printf("  %d\n", *i);
00377     }
00378   }
00379 
00380 
00381  private:
00382   ArgumentParser __argp;
00383   BlackBoard *__bb;
00384   PanTiltInterface *__ptu_if;
00385   float __resolution;
00386   RobotisRX28 *__rx28;
00387 };
00388 
00389 
00390 /** Config tool main.
00391  * @param argc argument count
00392  * @param argv arguments
00393  */
00394 int
00395 main(int argc, char **argv)
00396 {
00397   try {
00398     PTUJoystickControl ptuctrl(argc, argv);
00399     ptuctrl.run();
00400   } catch (Exception e) {
00401     printf("Running failed: %s\n\n", e.what());
00402     print_usage(argv[0]);
00403     exit(0);
00404   }
00405 
00406   return 0;
00407 }