Fawkes API Fawkes Development Version
|
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 }