00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00062
00063
00064 class JoystickQuitHandler : public SignalHandler
00065 {
00066 public:
00067
00068
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
00085
00086
00087 class JoystickBlackBoardPoster : public JoystickBlackBoardHandler
00088 {
00089 public:
00090
00091
00092
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
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
00163
00164
00165 class JoystickBlackBoardLogger
00166 : public BlackBoardInterfaceListener,
00167 public SignalHandler
00168 {
00169 public:
00170
00171
00172
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
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
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
00258
00259
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 }