Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
ffjoystick.cpp
1 
2 /***************************************************************************
3  * ffjoystick.cpp - Joystick app to provide a local joystick via a
4  * RemoteBlackBoard connection.
5  *
6  * Created: Sun Nov 23 01:19:54 2008
7  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "acquisition_thread.h"
25 #include "act_thread.h"
26 #include "remote_bb_poster.h"
27 
28 #include <core/threading/thread.h>
29 #include <core/threading/wait_condition.h>
30 #include <core/exceptions/system.h>
31 #include <netcomm/fawkes/client.h>
32 #include <blackboard/remote.h>
33 #include <blackboard/interface_listener.h>
34 #include <utils/system/argparser.h>
35 #include <utils/system/signal.h>
36 #include <logging/console.h>
37 #include <netcomm/fawkes/client_handler.h>
38 #include <netcomm/socket/socket.h>
39 
40 #include <cstring>
41 #include <cstdlib>
42 #include <cstdio>
43 #include <unistd.h>
44 #include <string>
45 
46 #include <interfaces/JoystickInterface.h>
47 
48 using namespace fawkes;
49 
50 bool quit = false;
51 
52 void
53 print_usage(const char *program_name)
54 {
55  printf("Usage: %s [-h] [-r host[:port]] [-d device] [-l]\n"
56  " -h This help message\n"
57  " -r host[:port] Remote host (and optionally port) to connect to\n"
58  " -d device Joystick device to use\n"
59  " -l Start in logging mode - print data read from bb\n",
60  program_name);
61 }
62 
63 /** Simple signal handler for ffjoystick.
64  * @author Tim Niemueller
65  */
67 {
68  public:
69  /** Constructor.
70  * @param aqt Joystick acquisition thread
71  */
73  : __aqt(aqt)
74  {
75  }
76 
77  virtual void handle_signal(int signal)
78  {
79  __aqt.cancel();
80  }
81 
82  private:
84 };
85 
86 /** Log joystick data gathered via RemoteBlackBoard to console.
87  * @author Tim Niemueller
88  */
91  public SignalHandler
92 {
93  public:
94  /** Constructor.
95  * @param argp argument parser
96  * @param logger logger
97  */
99  : BlackBoardInterfaceListener("JoystickBlackBoardLogger"),
100  __argp(argp), __logger(logger)
101  {
102  char *host = (char *)"localhost";
103  unsigned short int port = 1910;
104  bool free_host = argp.parse_hostport("r", &host, &port);
105 
106  __bb = new RemoteBlackBoard(host, port);
107  if ( free_host ) free(host);
108 
109  __joystick_if = __bb->open_for_reading<JoystickInterface>("Joystick");
110  __warning_printed = false;
111 
112  __joystick_if->read();
113  logger->log_debug("Joystick", "Number of Axes: %i",
114  __joystick_if->num_axes());
115  logger->log_debug("Joystick", "Number of Buttons: %i",
116  __joystick_if->num_buttons());
117 
118  /** Just a quick hack for rumble testing
119  __joystick_if->msgq_enqueue(
120  new JoystickInterface::StartRumbleMessage(1000, 0,
121  JoystickInterface::DIRECTION_UP,
122  0xFFFF, 0x8000));
123  */
124 
125  bbil_add_data_interface(__joystick_if);
126  __bb->register_listener(this);
127  }
128 
129  /** Destructor. */
131  {
132  __bb->close(__joystick_if);
133  delete __bb;
134  }
135 
136  virtual void bb_interface_data_changed(Interface *interface) throw()
137  {
138  if ( ! __bb->is_alive() ) {
139  if ( __bb->try_aliveness_restore() ) {
140  __logger->log_info("Joystick", "Connection re-established, writing data");
141  __warning_printed = false;
142  }
143  }
144 
145  try {
146  __joystick_if->read();
147  float *axis_value = __joystick_if->axis();
148  __logger->log_info("Joystick", "Axes: 0: %f 1: %f 2: %f 3: %f 4: %f "
149  "5: %f 6: %f 7: %f 8: %f",
150  axis_value[0], axis_value[1],
151  axis_value[2], axis_value[3],
152  axis_value[4], axis_value[5],
153  axis_value[6], axis_value[7]);
154  char button_string[33];
155  button_string[32] = 0;
156  unsigned int pressed_buttons = __joystick_if->pressed_buttons();
157  for (unsigned int i = 0; i < 32; ++i) {
158  button_string[i] = (pressed_buttons & (1 << i)) ? '1' : '0';
159  }
160  __logger->log_info("Joystick", "Buttons: %s", button_string);
161  } catch (Exception &e) {
162  if ( ! __warning_printed ) {
163  e.print_trace();
164  __logger->log_warn("Joystick", "Lost connection to BlackBoard, "
165  "will try to re-establish");
166  __warning_printed = true;
167  }
168  }
169  }
170 
171  void handle_signal(int signum)
172  {
173  __waitcond.wake_all();
174  }
175 
176  /** Wait for quit signal from signal handler. */
177  void run()
178  {
179  __waitcond.wait();
180  }
181 
182  private:
183  bool __warning_printed;
184  ArgumentParser &__argp;
185  BlackBoard *__bb;
186  Logger *__logger;
187  JoystickInterface *__joystick_if;
188  WaitCondition __waitcond;
189 };
190 
191 
192 /** Wake actuator thread on incomin messages.
193  * @author Tim Niemueller
194  */
197 {
198  public:
199  /** Constructor.
200  * @param aqt acquisition thread to pass to message processor
201  * @param blackboard blackboard to register for message event handling
202  * @param joystick_if joystick interface to listen on for messages
203  * @param logger logger
204  */
206  BlackBoard *blackboard,
207  JoystickInterface *joystick_if,
208  Logger *logger)
209  : BlackBoardInterfaceListener("JoystickBlackBoardActMsgProcThread"),
210  __bb(blackboard), __joystick_if(joystick_if),
211  __logger(logger)
212  {
213  __msgproc = new JoystickActThread::MessageProcessor(aqt, __joystick_if);
214  __msgproc->process();
215  bbil_add_message_interface(__joystick_if);
216  __bb->register_listener(this);
217  }
218 
219  /** Destructor. */
221  {
222  __bb->unregister_listener(this);
223  bbil_remove_message_interface(__joystick_if);
224  delete __msgproc;
225  }
226 
227  virtual bool bb_interface_message_received(Interface *interface,
228  Message *message) throw()
229  {
230  try {
231  __msgproc->process();
232  __msgproc->process_message(message);
233  } catch (Exception &e) {
234  e.print_trace();
235  }
236  return false;
237  }
238 
239  private:
241  BlackBoard *__bb;
242  JoystickInterface *__joystick_if;
243  Logger *__logger;
244 };
245 
246 /** Config tool main.
247  * @param argc argument count
248  * @param argv arguments
249  */
250 int
251 main(int argc, char **argv)
252 {
253  try
254  {
255  ArgumentParser argp(argc, argv, "hr:d:l");
256 
257  if ( argp.has_arg("h") ) {
258  print_usage(argv[0]);
259  exit(0);
260  }
261 
262  const char *joystick_device = "/dev/input/js0";
263  if ( argp.has_arg("d") ) {
264  joystick_device = argp.arg("d");
265  }
266 
267  ConsoleLogger logger;
268 
269  if ( argp.has_arg("l") ) {
270  JoystickBlackBoardLogger jbl(argp, &logger);
271  SignalManager::register_handler(SIGINT, &jbl);
272  jbl.run();
273  } else {
274  char *host = (char *)"localhost";
275  unsigned short int port = 1910;
276  bool free_host = argp.parse_hostport("r", &host, &port);
277 
278  JoystickRemoteBlackBoardPoster jbp(host, port, &logger);
279  JoystickAcquisitionThread aqt(joystick_device, &jbp, &logger);
280  JoystickBlackBoardActListener aml(&aqt, jbp.blackboard(),
281  jbp.joystick_if(), &logger);
282 
283  JoystickQuitHandler jqh(aqt);
284  SignalManager::register_handler(SIGINT, &jqh);
285 
286  if (free_host) free(host);
287 
288  aqt.start();
289  aqt.join();
290  }
291  }
292  catch (UnknownArgumentException e)
293  {
294  printf("Error: Unknown Argument\n\n");
295  print_usage(argv[0]);
296  exit(0);
297  }
298  catch (SocketException e)
299  {
300  printf("\nError: could not connect:\n%s\n", e.what());
301  }
302  catch (CouldNotOpenFileException e)
303  {
304  printf("\nError: could not open joystick device:\n%s\n", e.what());
305  }
306 
307  return 0;
308 }