Fawkes API  Fawkes Development Version
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 <blackboard/interface_listener.h>
29 #include <blackboard/remote.h>
30 #include <core/exceptions/system.h>
31 #include <core/threading/thread.h>
32 #include <core/threading/wait_condition.h>
33 #include <interfaces/JoystickInterface.h>
34 #include <logging/console.h>
35 #include <netcomm/fawkes/client.h>
36 #include <netcomm/fawkes/client_handler.h>
37 #include <netcomm/socket/socket.h>
38 #include <utils/system/argparser.h>
39 #include <utils/system/signal.h>
40 
41 #include <cstdio>
42 #include <cstdlib>
43 #include <cstring>
44 #include <string>
45 #include <unistd.h>
46 
47 using namespace fawkes;
48 
49 bool quit = false;
50 
51 void
52 print_usage(const char *program_name)
53 {
54  printf("Usage: %s [-h] [-r host[:port]] [-d device] [-l]\n"
55  " -h This help message\n"
56  " -r host[:port] Remote host (and optionally port) to connect to\n"
57  " -d device Joystick device to use\n"
58  " -l Start in logging mode - print data read from bb\n",
59  program_name);
60 }
61 
62 /** Simple signal handler for ffjoystick.
63  * @author Tim Niemueller
64  */
66 {
67 public:
68  /** Constructor.
69  * @param aqt Joystick acquisition thread
70  */
72  {
73  }
74 
75  virtual void
76  handle_signal(int signal)
77  {
78  aqt_.cancel();
79  }
80 
81 private:
83 };
84 
85 /** Log joystick data gathered via RemoteBlackBoard to console.
86  * @author Tim Niemueller
87  */
89 {
90 public:
91  /** Constructor.
92  * @param argp argument parser
93  * @param logger logger
94  */
96  : BlackBoardInterfaceListener("JoystickBlackBoardLogger"), logger_(logger)
97  {
98  char * host = (char *)"localhost";
99  unsigned short int port = 1910;
100  bool free_host = argp.parse_hostport("r", &host, &port);
101 
102  bb_ = new RemoteBlackBoard(host, port);
103  if (free_host)
104  free(host);
105 
106  joystick_if_ = bb_->open_for_reading<JoystickInterface>("Joystick");
107  warning_printed_ = false;
108 
109  joystick_if_->read();
110  logger->log_debug("Joystick", "Number of Axes: %i", joystick_if_->num_axes());
111  logger->log_debug("Joystick", "Number of Buttons: %i", joystick_if_->num_buttons());
112 
113  /** Just a quick hack for rumble testing
114  joystick_if_->msgq_enqueue(
115  new JoystickInterface::StartRumbleMessage(1000, 0,
116  JoystickInterface::DIRECTION_UP,
117  0xFFFF, 0x8000));
118  */
119 
120  bbil_add_data_interface(joystick_if_);
121  bb_->register_listener(this);
122  }
123 
124  /** Destructor. */
126  {
127  bb_->close(joystick_if_);
128  delete bb_;
129  }
130 
131  virtual void
133  {
134  if (!bb_->is_alive()) {
135  if (bb_->try_aliveness_restore()) {
136  logger_->log_info("Joystick", "Connection re-established, writing data");
137  warning_printed_ = false;
138  }
139  }
140 
141  try {
142  joystick_if_->read();
143  float *axis_value = joystick_if_->axis();
144  logger_->log_info("Joystick",
145  "Axes: 0: %f 1: %f 2: %f 3: %f 4: %f "
146  "5: %f 6: %f 7: %f",
147  axis_value[0],
148  axis_value[1],
149  axis_value[2],
150  axis_value[3],
151  axis_value[4],
152  axis_value[5],
153  axis_value[6],
154  axis_value[7]);
155  char button_string[33];
156  button_string[32] = 0;
157  unsigned int pressed_buttons = joystick_if_->pressed_buttons();
158  for (unsigned int i = 0; i < 32; ++i) {
159  button_string[i] = (pressed_buttons & (1 << i)) ? '1' : '0';
160  }
161  logger_->log_info("Joystick", "Buttons: %s", button_string);
162  } catch (Exception &e) {
163  if (!warning_printed_) {
164  e.print_trace();
165  logger_->log_warn("Joystick",
166  "Lost connection to BlackBoard, "
167  "will try to re-establish");
168  warning_printed_ = true;
169  }
170  }
171  }
172 
173  void
174  handle_signal(int signum)
175  {
176  waitcond_.wake_all();
177  }
178 
179  /** Wait for quit signal from signal handler. */
180  void
181  run()
182  {
183  waitcond_.wait();
184  }
185 
186 private:
187  bool warning_printed_;
188  BlackBoard * bb_;
189  Logger * logger_;
190  JoystickInterface *joystick_if_;
191  WaitCondition waitcond_;
192 };
193 
194 /** Wake actuator thread on incomin messages.
195  * @author Tim Niemueller
196  */
198 {
199 public:
200  /** Constructor.
201  * @param aqt acquisition thread to pass to message processor
202  * @param blackboard blackboard to register for message event handling
203  * @param joystick_if joystick interface to listen on for messages
204  * @param logger logger
205  */
207  BlackBoard * blackboard,
208  JoystickInterface * joystick_if,
209  Logger * logger)
210  : BlackBoardInterfaceListener("JoystickBlackBoardActMsgProcThread"),
211  bb_(blackboard),
212  joystick_if_(joystick_if)
213  {
214  msgproc_ = new JoystickActThread::MessageProcessor(aqt, joystick_if_);
215  msgproc_->process();
216  bbil_add_message_interface(joystick_if_);
217  bb_->register_listener(this);
218  }
219 
220  /** Destructor. */
222  {
223  bb_->unregister_listener(this);
224  bbil_remove_message_interface(joystick_if_);
225  delete msgproc_;
226  }
227 
228  virtual bool
229  bb_interface_message_received(Interface *interface, Message *message) throw()
230  {
231  try {
232  msgproc_->process();
233  msgproc_->process_message(message);
234  } catch (Exception &e) {
235  e.print_trace();
236  }
237  return false;
238  }
239 
240 private:
242  BlackBoard * bb_;
243  JoystickInterface * joystick_if_;
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  ArgumentParser argp(argc, argv, "hr:d:l");
255 
256  if (argp.has_arg("h")) {
257  print_usage(argv[0]);
258  exit(0);
259  }
260 
261  const char *joystick_device = "/dev/input/js0";
262  if (argp.has_arg("d")) {
263  joystick_device = argp.arg("d");
264  }
265 
266  ConsoleLogger logger;
267 
268  if (argp.has_arg("l")) {
269  JoystickBlackBoardLogger jbl(argp, &logger);
270  SignalManager::register_handler(SIGINT, &jbl);
271  jbl.run();
272  } else {
273  char * host = (char *)"localhost";
274  unsigned short int port = 1910;
275  bool free_host = argp.parse_hostport("r", &host, &port);
276 
277  JoystickRemoteBlackBoardPoster jbp(host, port, &logger);
278  JoystickAcquisitionThread aqt(joystick_device, &jbp, &logger);
279  JoystickBlackBoardActListener aml(&aqt, jbp.blackboard(), jbp.joystick_if(), &logger);
280 
281  JoystickQuitHandler jqh(aqt);
282  SignalManager::register_handler(SIGINT, &jqh);
283 
284  if (free_host)
285  free(host);
286 
287  aqt.start();
288  aqt.join();
289  }
290  } catch (UnknownArgumentException &e) {
291  printf("Error: Unknown Argument\n\n");
292  print_usage(argv[0]);
293  exit(0);
294  } catch (SocketException &e) {
295  printf("\nError: could not connect:\n%s\n", e.what());
296  } catch (CouldNotOpenFileException &e) {
297  printf("\nError: could not open joystick device:\n%s\n", e.what());
298  }
299 
300  return 0;
301 }
fawkes::ArgumentParser::parse_hostport
bool parse_hostport(const char *argn, char **host, unsigned short int *port)
Parse host:port string.
Definition: argparser.cpp:228
fawkes::SocketException
Definition: socket.h:60
JoystickBlackBoardLogger::bb_interface_data_changed
virtual void bb_interface_data_changed(Interface *interface)
BlackBoard data changed notification.
Definition: ffjoystick.cpp:132
fawkes::ConsoleLogger
Definition: console.h:40
JoystickBlackBoardActListener::bb_interface_message_received
virtual bool bb_interface_message_received(Interface *interface, Message *message)
BlackBoard message received notification.
Definition: ffjoystick.cpp:229
fawkes::WaitCondition
Definition: wait_condition.h:40
fawkes::Message
Definition: message.h:40
fawkes::SignalHandler
Definition: signal.h:43
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:475
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
fawkes::BlackBoard
Definition: blackboard.h:48
fawkes::UnknownArgumentException
Thrown if unknown argument was supplied.
Definition: argparser.h:42
JoystickAcquisitionThread
Definition: acquisition_thread.h:41
fawkes::RemoteBlackBoard
Definition: remote.h:51
JoystickBlackBoardLogger
Log joystick data gathered via RemoteBlackBoard to console.
Definition: ffjoystick.cpp:88
JoystickBlackBoardLogger::handle_signal
void handle_signal(int signum)
Definition: ffjoystick.cpp:174
JoystickBlackBoardLogger::JoystickBlackBoardLogger
JoystickBlackBoardLogger(ArgumentParser &argp, Logger *logger)
Constructor.
Definition: ffjoystick.cpp:95
fawkes::SignalManager::register_handler
static SignalHandler * register_handler(int signum, SignalHandler *handler)
Register a SignalHandler for a signal.
Definition: signal.cpp:121
JoystickQuitHandler::JoystickQuitHandler
JoystickQuitHandler(JoystickAcquisitionThread &aqt)
Constructor.
Definition: ffjoystick.cpp:71
fawkes::CouldNotOpenFileException
Definition: system.h:56
fawkes::MultiLogger::log_debug
virtual void log_debug(const char *component, const char *format,...)
Definition: multi.cpp:178
JoystickBlackBoardActListener::JoystickBlackBoardActListener
JoystickBlackBoardActListener(JoystickAcquisitionThread *aqt, BlackBoard *blackboard, JoystickInterface *joystick_if, Logger *logger)
Constructor.
Definition: ffjoystick.cpp:206
JoystickBlackBoardActListener
Wake actuator thread on incomin messages.
Definition: ffjoystick.cpp:197
fawkes::Logger
Definition: logger.h:40
fawkes
fawkes::ArgumentParser
Definition: argparser.h:67
fawkes::Interface
Definition: interface.h:77
fawkes::Exception::print_trace
void print_trace()
Prints trace to stderr.
Definition: exception.cpp:600
JoystickQuitHandler::handle_signal
virtual void handle_signal(int signal)
Definition: ffjoystick.cpp:76
JoystickActThread::MessageProcessor
Definition: act_thread.h:45
JoystickRemoteBlackBoardPoster
Definition: remote_bb_poster.h:35
JoystickQuitHandler
Simple signal handler for ffjoystick.
Definition: ffjoystick.cpp:65
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:638
JoystickBlackBoardLogger::~JoystickBlackBoardLogger
~JoystickBlackBoardLogger()
Destructor.
Definition: ffjoystick.cpp:125
JoystickBlackBoardLogger::run
void run()
Wait for quit signal from signal handler.
Definition: ffjoystick.cpp:181
fawkes::JoystickInterface
Definition: JoystickInterface.h:37
JoystickBlackBoardActListener::~JoystickBlackBoardActListener
~JoystickBlackBoardActListener()
Destructor.
Definition: ffjoystick.cpp:221
fawkes::Exception
Definition: exception.h:39