ffjoystick.cpp

00001 
00002 /***************************************************************************
00003  *  ffjoystick.cpp - Joystick app to provide a local joystick via a
00004  *                   RemoteBlackBoard connection.
00005  *
00006  *  Created: Sun Nov 23 01:19:54 2008
00007  *  Copyright  2006-2008  Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 #include "acquisition_thread.h"
00025 
00026 #include <core/threading/thread.h>
00027 #include <core/threading/wait_condition.h>
00028 #include <core/exceptions/system.h>
00029 #include <netcomm/fawkes/client.h>
00030 #include <blackboard/remote.h>
00031 #include <blackboard/interface_listener.h>
00032 #include <utils/system/argparser.h>
00033 #include <utils/system/signal.h>
00034 #include <utils/logging/console.h>
00035 #include <netcomm/fawkes/client_handler.h>
00036 #include <netcomm/socket/socket.h>
00037 
00038 #include <cstring>
00039 #include <cstdlib>
00040 #include <cstdio>
00041 #include <unistd.h>
00042 #include <string>
00043 
00044 #include <interfaces/JoystickInterface.h>
00045 
00046 using namespace fawkes;
00047 
00048 bool quit = false;
00049 
00050 void
00051 print_usage(const char *program_name)
00052 {
00053   printf("Usage: %s [-h] [-r host[:port]] [-d device] [-l]\n"
00054          " -h              This help message\n"
00055          " -r host[:port]  Remote host (and optionally port) to connect to\n"
00056          " -d device       Joystick device to use\n"
00057          " -l              Start in logging mode - print data read from bb\n",
00058          program_name);
00059 }
00060 
00061 /** Simple signal handler for ffjoystick.
00062  * @author Tim Niemueller
00063  */
00064 class JoystickQuitHandler : public SignalHandler
00065 {
00066  public:
00067   /** Constructor.
00068    * @param aqt Joystick acquisition thread
00069    */
00070   JoystickQuitHandler(JoystickAcquisitionThread &aqt)
00071     : __aqt(aqt)
00072   {
00073   }
00074 
00075   virtual void handle_signal(int signal)
00076   {
00077     __aqt.cancel();
00078   }
00079 
00080  private:
00081   JoystickAcquisitionThread &__aqt;
00082 };
00083 
00084 /** Small glue class that posts new data to a RemoteBlackBoard.
00085  * @author Tim Niemueller
00086  */
00087 class JoystickBlackBoardPoster : public JoystickBlackBoardHandler
00088 {
00089  public:
00090   /** Constructor.
00091    * @param argp argument parser, makes use of -r flag for host:port.
00092    * @param logger logger
00093    */
00094   JoystickBlackBoardPoster(ArgumentParser &argp, Logger *logger)
00095     : __argp(argp), __logger(logger)
00096   {
00097     char *host = (char *)"localhost";
00098     unsigned short int port = 1910;
00099     bool free_host = argp.parse_hostport("r", &host, &port);
00100     
00101     __bb = new RemoteBlackBoard(host, port);
00102     if ( free_host )  free(host);
00103 
00104     __joystick_if = __bb->open_for_writing<JoystickInterface>("Joystick");
00105     __warning_printed = false;
00106   }
00107 
00108   /** Destructor. */
00109   ~JoystickBlackBoardPoster()
00110   {
00111     __bb->close(__joystick_if);
00112     delete __bb;
00113   }
00114 
00115   virtual void joystick_changed(unsigned int pressed_buttons,
00116                                 float *axis_x_values, float *axis_y_values)
00117   {
00118     if ( ! __bb->is_alive() ) {
00119       if ( __bb->try_aliveness_restore() ) {
00120         __logger->log_info("Joystick", "Connection re-established, writing data");
00121         __warning_printed = false;
00122       }
00123     }
00124 
00125     try {
00126       __joystick_if->set_pressed_buttons( pressed_buttons );
00127       __joystick_if->set_axis_x( axis_x_values );
00128       __joystick_if->set_axis_y( axis_y_values );
00129       __joystick_if->write();
00130     } catch (Exception &e) {
00131       if ( ! __warning_printed ) {
00132         e.print_trace();
00133         __logger->log_warn("Joystick", "Lost connection to BlackBoard, will try to re-establish");
00134         __warning_printed = true;
00135       }
00136     }
00137   }
00138 
00139   void joystick_plugged(char num_axes, char num_buttons)
00140   {
00141     __joystick_if->set_num_axes( num_axes );
00142     __joystick_if->set_num_buttons( num_buttons );
00143     __joystick_if->write();
00144   }
00145 
00146   void joystick_unplugged()
00147   {
00148     __joystick_if->set_num_axes( 0 );
00149     __joystick_if->set_num_buttons( 0 );
00150     __joystick_if->write();
00151   }
00152 
00153  private:
00154   bool __warning_printed;
00155   ArgumentParser &__argp;
00156   BlackBoard *__bb;
00157   JoystickInterface *__joystick_if;
00158   Logger *__logger;
00159 };
00160 
00161 
00162 /** Log joystick data gathered via RemoteBlackBoard to console.
00163  * @author Tim Niemueller
00164  */
00165 class JoystickBlackBoardLogger
00166   : public BlackBoardInterfaceListener,
00167     public SignalHandler
00168 {
00169  public:
00170   /** Constructor.
00171    * @param argp argument parser
00172    * @param logger logger
00173    */
00174   JoystickBlackBoardLogger(ArgumentParser &argp, Logger *logger)
00175     :  BlackBoardInterfaceListener("JoystickBlackBoardLogger"),
00176        __argp(argp), __logger(logger)
00177   {
00178     char *host = (char *)"localhost";
00179     unsigned short int port = 1910;
00180     bool free_host = argp.parse_hostport("r", &host, &port);
00181     
00182     __bb = new RemoteBlackBoard(host, port);
00183     if ( free_host )  free(host);
00184 
00185     __joystick_if = __bb->open_for_reading<JoystickInterface>("Joystick");
00186     __warning_printed = false;
00187 
00188     __joystick_if->read();
00189     logger->log_debug("Joystick", "Number of Axes:    %i", __joystick_if->num_axes());
00190     logger->log_debug("Joystick", "Number of Buttons: %i", __joystick_if->num_buttons());
00191 
00192     bbil_add_data_interface(__joystick_if);
00193     __bb->register_listener(this, BlackBoard::BBIL_FLAG_DATA);
00194   }
00195 
00196   /** Destructor. */
00197   ~JoystickBlackBoardLogger()
00198   {
00199     __bb->close(__joystick_if);
00200     delete __bb;
00201   }
00202 
00203   virtual void bb_interface_data_changed(Interface *interface) throw()
00204   {
00205     if ( ! __bb->is_alive() ) {
00206       if ( __bb->try_aliveness_restore() ) {
00207         __logger->log_info("Joystick", "Connection re-established, writing data");
00208         __warning_printed = false;
00209       }
00210     }
00211 
00212     try {
00213       __joystick_if->read();
00214       float *axis_x_value = __joystick_if->axis_x();
00215       float *axis_y_value = __joystick_if->axis_y();
00216       __logger->log_info("Joystick", "0: (%f, %f)  1: (%f, %f)  2: (%f, %f)  3: (%f, %f)",
00217                          axis_x_value[0], axis_y_value[0],
00218                          axis_x_value[1], axis_y_value[1],
00219                          axis_x_value[2], axis_y_value[2],
00220                          axis_x_value[3], axis_y_value[3]);
00221       char button_string[33];
00222       button_string[32] = 0;
00223       unsigned int pressed_buttons = __joystick_if->pressed_buttons();
00224       for (unsigned int i = 0; i < 32; ++i) {
00225         button_string[i] = (pressed_buttons & (1 << i)) ? '1' : '0';
00226       }
00227       __logger->log_info("Joystick", "Buttons: %s", button_string);
00228     } catch (Exception &e) {
00229       if ( ! __warning_printed ) {
00230         e.print_trace();
00231         __logger->log_warn("Joystick", "Lost connection to BlackBoard, will try to re-establish");
00232         __warning_printed = true;
00233       }
00234     }
00235   }
00236 
00237   void handle_signal(int signum)
00238   {
00239     __waitcond.wake_all();
00240   }
00241 
00242   /** Wait for quit signal from signal handler. */
00243   void run()
00244   {
00245     __waitcond.wait();
00246   }
00247 
00248  private:
00249   bool __warning_printed;
00250   ArgumentParser &__argp;
00251   BlackBoard *__bb;
00252   Logger *__logger;
00253   JoystickInterface *__joystick_if;
00254   WaitCondition __waitcond;
00255 };
00256 
00257 /** Config tool main.
00258  * @param argc argument count
00259  * @param argv arguments
00260  */
00261 int
00262 main(int argc, char **argv)
00263 {
00264   try
00265   {
00266     ArgumentParser argp(argc, argv, "hr:d:l");
00267     
00268     if ( argp.has_arg("h") ) {
00269       print_usage(argv[0]);
00270       exit(0);
00271     }
00272 
00273     const char *joystick_device = "/dev/input/js0";
00274     if ( argp.has_arg("d") ) {
00275       joystick_device = argp.arg("d");
00276     }
00277 
00278     ConsoleLogger logger;
00279 
00280     if ( argp.has_arg("l") ) {
00281       JoystickBlackBoardLogger jbl(argp, &logger);
00282       SignalManager::register_handler(SIGINT, &jbl);
00283       jbl.run();
00284     } else {
00285       JoystickBlackBoardPoster jbp(argp, &logger);
00286       JoystickAcquisitionThread aqt(joystick_device, &jbp, &logger);
00287 
00288       JoystickQuitHandler jqh(aqt);
00289       SignalManager::register_handler(SIGINT, &jqh);
00290 
00291       aqt.start();
00292       aqt.join();
00293     }
00294   }
00295   catch (UnknownArgumentException e)
00296   {
00297     printf("Error: Unknown Argument\n\n");
00298     print_usage(argv[0]);
00299     exit(0);
00300   }
00301   catch (SocketException e)
00302   {
00303     printf("\nError: could not connect:\n%s\n", e.what());
00304   }
00305   catch (CouldNotOpenFileException e)
00306   {
00307     printf("\nError: could not open joystick device:\n%s\n", e.what());
00308   }
00309 
00310   return 0;
00311 }

Generated on Tue Feb 22 13:31:28 2011 for Fawkes API by  doxygen 1.4.7