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-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 }