Fawkes API  Fawkes Development Version
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-2011  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 #include "act_thread.h"
00026 #include "remote_bb_poster.h"
00027 
00028 #include <core/threading/thread.h>
00029 #include <core/threading/wait_condition.h>
00030 #include <core/exceptions/system.h>
00031 #include <netcomm/fawkes/client.h>
00032 #include <blackboard/remote.h>
00033 #include <blackboard/interface_listener.h>
00034 #include <utils/system/argparser.h>
00035 #include <utils/system/signal.h>
00036 #include <logging/console.h>
00037 #include <netcomm/fawkes/client_handler.h>
00038 #include <netcomm/socket/socket.h>
00039 
00040 #include <cstring>
00041 #include <cstdlib>
00042 #include <cstdio>
00043 #include <unistd.h>
00044 #include <string>
00045 
00046 #include <interfaces/JoystickInterface.h>
00047 
00048 using namespace fawkes;
00049 
00050 bool quit = false;
00051 
00052 void
00053 print_usage(const char *program_name)
00054 {
00055   printf("Usage: %s [-h] [-r host[:port]] [-d device] [-l]\n"
00056          " -h              This help message\n"
00057          " -r host[:port]  Remote host (and optionally port) to connect to\n"
00058          " -d device       Joystick device to use\n"
00059          " -l              Start in logging mode - print data read from bb\n",
00060          program_name);
00061 }
00062 
00063 /** Simple signal handler for ffjoystick.
00064  * @author Tim Niemueller
00065  */
00066 class JoystickQuitHandler : public SignalHandler
00067 {
00068  public:
00069   /** Constructor.
00070    * @param aqt Joystick acquisition thread
00071    */
00072   JoystickQuitHandler(JoystickAcquisitionThread &aqt)
00073     : __aqt(aqt)
00074   {
00075   }
00076 
00077   virtual void handle_signal(int signal)
00078   {
00079     __aqt.cancel();
00080   }
00081 
00082  private:
00083   JoystickAcquisitionThread &__aqt;
00084 };
00085 
00086 /** Log joystick data gathered via RemoteBlackBoard to console.
00087  * @author Tim Niemueller
00088  */
00089 class JoystickBlackBoardLogger
00090   : public BlackBoardInterfaceListener,
00091     public SignalHandler
00092 {
00093  public:
00094   /** Constructor.
00095    * @param argp argument parser
00096    * @param logger logger
00097    */
00098   JoystickBlackBoardLogger(ArgumentParser &argp, Logger *logger)
00099     :  BlackBoardInterfaceListener("JoystickBlackBoardLogger"),
00100        __argp(argp), __logger(logger)
00101   {
00102     char *host = (char *)"localhost";
00103     unsigned short int port = 1910;
00104     bool free_host = argp.parse_hostport("r", &host, &port);
00105     
00106     __bb = new RemoteBlackBoard(host, port);
00107     if ( free_host )  free(host);
00108 
00109     __joystick_if = __bb->open_for_reading<JoystickInterface>("Joystick");
00110     __warning_printed = false;
00111 
00112     __joystick_if->read();
00113     logger->log_debug("Joystick", "Number of Axes:    %i",
00114                       __joystick_if->num_axes());
00115     logger->log_debug("Joystick", "Number of Buttons: %i",
00116                       __joystick_if->num_buttons());
00117 
00118     /** Just a quick hack for rumble testing
00119       __joystick_if->msgq_enqueue(
00120         new JoystickInterface::StartRumbleMessage(1000, 0,
00121                                                   JoystickInterface::DIRECTION_UP,
00122                                                   0xFFFF, 0x8000));
00123     */
00124 
00125     bbil_add_data_interface(__joystick_if);
00126     __bb->register_listener(this);
00127   }
00128 
00129   /** Destructor. */
00130   ~JoystickBlackBoardLogger()
00131   {
00132     __bb->close(__joystick_if);
00133     delete __bb;
00134   }
00135 
00136   virtual void bb_interface_data_changed(Interface *interface) throw()
00137   {
00138     if ( ! __bb->is_alive() ) {
00139       if ( __bb->try_aliveness_restore() ) {
00140         __logger->log_info("Joystick", "Connection re-established, writing data");
00141         __warning_printed = false;
00142       }
00143     }
00144 
00145     try {
00146       __joystick_if->read();
00147       float *axis_value = __joystick_if->axis();
00148       __logger->log_info("Joystick", "Axes:    0: %f  1: %f  2: %f  3: %f  4: %f  "
00149                          "5: %f  6: %f  7: %f  8: %f",
00150                          axis_value[0], axis_value[1],
00151                          axis_value[2], axis_value[3],
00152                          axis_value[4], axis_value[5],
00153                          axis_value[6], axis_value[7]);
00154       char button_string[33];
00155       button_string[32] = 0;
00156       unsigned int pressed_buttons = __joystick_if->pressed_buttons();
00157       for (unsigned int i = 0; i < 32; ++i) {
00158         button_string[i] = (pressed_buttons & (1 << i)) ? '1' : '0';
00159       }
00160       __logger->log_info("Joystick", "Buttons: %s", button_string);
00161     } catch (Exception &e) {
00162       if ( ! __warning_printed ) {
00163         e.print_trace();
00164         __logger->log_warn("Joystick", "Lost connection to BlackBoard, "
00165                            "will try to re-establish");
00166         __warning_printed = true;
00167       }
00168     }
00169   }
00170 
00171   void handle_signal(int signum)
00172   {
00173     __waitcond.wake_all();
00174   }
00175 
00176   /** Wait for quit signal from signal handler. */
00177   void run()
00178   {
00179     __waitcond.wait();
00180   }
00181 
00182  private:
00183   bool __warning_printed;
00184   ArgumentParser &__argp;
00185   BlackBoard *__bb;
00186   Logger *__logger;
00187   JoystickInterface *__joystick_if;
00188   WaitCondition __waitcond;
00189 };
00190 
00191 
00192 /** Wake actuator thread on incomin messages.
00193  * @author Tim Niemueller
00194  */
00195 class JoystickBlackBoardActListener
00196   : public BlackBoardInterfaceListener
00197 {
00198  public:
00199   /** Constructor.
00200    * @param aqt acquisition thread to pass to message processor
00201    * @param blackboard blackboard to register for message event handling
00202    * @param joystick_if joystick interface to listen on for messages
00203    * @param logger logger
00204    */
00205   JoystickBlackBoardActListener(JoystickAcquisitionThread *aqt,
00206                                 BlackBoard *blackboard,
00207                                 JoystickInterface *joystick_if,
00208                                 Logger *logger)
00209     :  BlackBoardInterfaceListener("JoystickBlackBoardActMsgProcThread"),
00210        __bb(blackboard), __joystick_if(joystick_if),
00211        __logger(logger)
00212   {
00213     __msgproc = new JoystickActThread::MessageProcessor(aqt, __joystick_if);
00214     __msgproc->process();
00215     bbil_add_message_interface(__joystick_if);
00216     __bb->register_listener(this);
00217   }
00218 
00219   /** Destructor. */
00220   ~JoystickBlackBoardActListener()
00221   {
00222     __bb->unregister_listener(this);
00223     bbil_remove_message_interface(__joystick_if);
00224     delete __msgproc;
00225   }
00226 
00227   virtual bool bb_interface_message_received(Interface *interface,
00228                                              Message *message) throw()
00229   {
00230     try {
00231       __msgproc->process();
00232       __msgproc->process_message(message);
00233     } catch (Exception &e) {
00234       e.print_trace();
00235     }
00236     return false;
00237   }
00238 
00239  private:
00240   JoystickActThread::MessageProcessor *__msgproc;
00241   BlackBoard *__bb;
00242   JoystickInterface *__joystick_if;
00243   Logger *__logger;
00244 };
00245 
00246 /** Config tool main.
00247  * @param argc argument count
00248  * @param argv arguments
00249  */
00250 int
00251 main(int argc, char **argv)
00252 {
00253   try
00254   {
00255     ArgumentParser argp(argc, argv, "hr:d:l");
00256     
00257     if ( argp.has_arg("h") ) {
00258       print_usage(argv[0]);
00259       exit(0);
00260     }
00261 
00262     const char *joystick_device = "/dev/input/js0";
00263     if ( argp.has_arg("d") ) {
00264       joystick_device = argp.arg("d");
00265     }
00266 
00267     ConsoleLogger logger;
00268 
00269     if ( argp.has_arg("l") ) {
00270       JoystickBlackBoardLogger jbl(argp, &logger);
00271       SignalManager::register_handler(SIGINT, &jbl);
00272       jbl.run();
00273     } else {
00274       char *host = (char *)"localhost";
00275       unsigned short int port = 1910;
00276       bool free_host = argp.parse_hostport("r", &host, &port);
00277 
00278       JoystickRemoteBlackBoardPoster jbp(host, port, &logger);
00279       JoystickAcquisitionThread aqt(joystick_device, &jbp, &logger);
00280       JoystickBlackBoardActListener aml(&aqt, jbp.blackboard(),
00281                                         jbp.joystick_if(), &logger);
00282 
00283       JoystickQuitHandler jqh(aqt);
00284       SignalManager::register_handler(SIGINT, &jqh);
00285 
00286       if (free_host)  free(host);
00287 
00288       aqt.start();
00289       aqt.join();
00290     }
00291   }
00292   catch (UnknownArgumentException e)
00293   {
00294     printf("Error: Unknown Argument\n\n");
00295     print_usage(argv[0]);
00296     exit(0);
00297   }
00298   catch (SocketException e)
00299   {
00300     printf("\nError: could not connect:\n%s\n", e.what());
00301   }
00302   catch (CouldNotOpenFileException e)
00303   {
00304     printf("\nError: could not open joystick device:\n%s\n", e.what());
00305   }
00306 
00307   return 0;
00308 }