Fawkes API  Fawkes Development Version
ffptu.cpp
1 
2 /***************************************************************************
3  * ffptu.cpp - Control PTU via keyboard
4  *
5  * Created: Thu Oct 06 16:28:16 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "../robotis/rx28.h"
24 
25 #include <blackboard/remote.h>
26 #include <core/exceptions/system.h>
27 #include <interface/interface_info.h>
28 #include <interfaces/PanTiltInterface.h>
29 #include <logging/console.h>
30 #include <utils/system/argparser.h>
31 #include <utils/system/getkey.h>
32 #include <utils/time/time.h>
33 
34 #include <cstdio>
35 #include <cstdlib>
36 #include <cstring>
37 #include <unistd.h>
38 
39 using namespace fawkes;
40 
41 void
42 print_usage(const char *program_name)
43 {
44  printf("Usage: %s [-h] [-r host[:port]] <command>\n"
45  " Options:\n"
46  " -h This help message\n"
47  " -r host[:port] Remote host (and optionally port) to connect to\n"
48  " -p <ptu> PTU, interface must have id 'PanTilt <ptu>'\n"
49  " -l List available PTUs (only on remote blackboard)\n"
50  " -i Invert tilt control buttons\n\n"
51  " Commands:\n"
52  "move [pan P] [tilt T]\n"
53  " Move PTU, if either pan or tilt are supplied, send the\n"
54  " command and wait for angle to be reached. Otherwise enter\n"
55  " interactive mode\n"
56  "rx28-set-id <old-id> <new-id>\n"
57  " Send message to set a new servo ID.\n"
58  " WARNING: use this with only a single servo connected!\n"
59  "rx28-discover\n"
60  " Discover and print servos on the BUS.\n",
61  program_name);
62 }
63 
64 /** Remote control PTUs via keyboard.
65  * @author Tim Niemueller
66  */
68 {
69 public:
70  /** Constructor.
71  * @param argc number of arguments in argv
72  * @param argv array of parameters passed into the program
73  */
74  PTUJoystickControl(int argc, char **argv) : argp_(argc, argv, "hr:p:li")
75  {
76  rx28_ = NULL;
77  bb_ = NULL;
78  ptu_if_ = NULL;
79  resolution_ = 0.1f;
80 
81  if (argp_.has_arg("h")) {
82  print_usage(argv[0]);
83  exit(0);
84  }
85  }
86 
87  /** Destructor. */
89  {
90  if (bb_) {
91  bb_->close(ptu_if_);
92  delete bb_;
93  }
94  delete rx28_;
95  }
96 
97  /** Initialize BB connection. */
98  void
100  {
101  char * host = (char *)"localhost";
102  unsigned short int port = 1910;
103  bool free_host = argp_.parse_hostport("r", &host, &port);
104 
105  bb_ = new RemoteBlackBoard(host, port);
106  if (free_host)
107  free(host);
108 
109  if (argp_.has_arg("l")) {
110  InterfaceInfoList *l = bb_->list("PanTiltInterface", "PanTilt *");
111  if (l->empty()) {
112  printf("No interfaces found");
113  }
114  for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
115  std::string id = i->id();
116  id = id.substr(std::string("PanTilt ").length());
117 
118  printf("PTU: %s Writer: %s\n", id.c_str(), i->has_writer() ? "Yes" : "No");
119  }
120  delete l;
121  return;
122  }
123 
124  if (argp_.has_arg("p")) {
125  std::string iface_id = std::string("PanTilt ") + argp_.arg("p");
126  ptu_if_ = bb_->open_for_reading<PanTiltInterface>(iface_id.c_str());
127  } else {
128  InterfaceInfoList *l = bb_->list("PanTiltInterface", "PanTilt *");
129  if (l->empty()) {
130  throw Exception("No PanTilt interface opened on remote!");
131  }
132  for (InterfaceInfoList::iterator i = l->begin(); i != l->end(); ++i) {
133  if (i->has_writer()) {
134  ptu_if_ = bb_->open_for_reading<PanTiltInterface>(i->id());
135  } else {
136  printf("Interface %s has no writer, ignoring\n", i->id());
137  }
138  }
139  delete l;
140  }
141 
142  if (!ptu_if_) {
143  throw Exception("No suitable PanTiltInterface found");
144  }
145  }
146 
147  /** Initialize Robotis RX28 raw servo access. */
148  void
150  {
151  rx28_ = new RobotisRX28("/dev/ttyUSB0");
152  RobotisRX28::DeviceList devl = rx28_->discover();
153 
154  if (devl.empty()) {
155  throw Exception("No devices found\n");
156  }
157  }
158 
159  /** Run control loop. */
160  void
161  run()
162  {
163  if (argp_.has_arg("l")) {
164  init_bb();
165  } else if (argp_.num_items() == 0) {
166  interactive_move();
167  } else if (strcmp(argp_.items()[0], "move") == 0) {
168  if (argp_.num_items() == 1) {
169  interactive_move();
170  } else {
171  exec_move();
172  }
173  } else if (strcmp(argp_.items()[0], "rx28-set-id") == 0) {
174  rx28_set_id();
175 
176  } else if (strcmp(argp_.items()[0], "rx28-discover") == 0) {
177  rx28_discover();
178 
179  } else {
180  printf("Unknown command '%s'\n", argp_.items()[0]);
181  }
182  }
183 
184 private:
185  void
186  interactive_move()
187  {
188  init_bb();
189 
190  Time last, now;
191 
192  char tilt_up = 65;
193  char tilt_down = 66;
194  if (argp_.has_arg("i"))
195  std::swap(tilt_up, tilt_down);
196 
197  float pan, tilt, new_pan, new_tilt;
198  float speed = 0.0, new_speed = 0.5;
199 
200  ptu_if_->read();
201  pan = new_pan = ptu_if_->pan();
202  tilt = new_tilt = ptu_if_->tilt();
203 
204  last.stamp();
205  char key = 0;
206  int wait_time = 5;
207  while (key != 'q') {
208  key = getkey(wait_time);
209  //printf("Key: %u = %u\n", key, key);
210  if (key != 0) {
211  now.stamp();
212  if ((now - &last) < 0.5) {
213  wait_time = 1;
214  }
215  last.stamp();
216  }
217  if (key == 0) {
218  wait_time = 5;
219 
220  } else if (key == 27) {
221  key = getkey();
222  if (key == 0) {
223  // Escape key
224  new_pan = pan;
225  new_tilt = tilt;
226  } else {
227  if (key != 91)
228  continue;
229 
230  key = getkey();
231  if (key == 0)
232  continue;
233 
234  if (key == tilt_up) {
235  new_tilt = std::min(tilt + resolution_, ptu_if_->max_tilt());
236  } else if (key == tilt_down) {
237  new_tilt = std::max(tilt - resolution_, ptu_if_->min_tilt());
238  } else if (key == 67) {
239  new_pan = std::max(pan - resolution_, ptu_if_->min_pan());
240  } else if (key == 68) {
241  new_pan = std::min(pan + resolution_, ptu_if_->max_pan());
242  } else
243  continue;
244  }
245  } else if (key == '0') {
246  new_pan = new_tilt = 0.f;
247  } else if (key == '9') {
248  new_pan = 0;
249  new_tilt = M_PI / 2.;
250  } else if (key == 'r') {
251  resolution_ = 0.1f;
252  } else if (key == 'R') {
253  resolution_ = 0.01f;
254  } else if (key == '+') {
255  new_speed = std::min(speed + 0.1, 1.0);
256  } else if (key == '-') {
257  new_speed = std::max(speed - 0.1, 0.0);
258  }
259 
260  if (speed != new_speed) {
261  speed = new_speed;
262  float pan_vel = speed * ptu_if_->max_pan_velocity();
263  float tilt_vel = speed * ptu_if_->max_tilt_velocity();
264 
265  printf("Setting velocity %f/%f (max %f/%f)\n",
266  pan_vel,
267  tilt_vel,
268  ptu_if_->max_pan_velocity(),
269  ptu_if_->max_tilt_velocity());
270 
272  new PanTiltInterface::SetVelocityMessage(pan_vel, tilt_vel);
273  ptu_if_->msgq_enqueue(svm);
274  }
275 
276  if ((pan != new_pan) || (tilt != new_tilt)) {
277  pan = new_pan;
278  tilt = new_tilt;
279 
280  printf("Goto %f/%f\n", pan, tilt);
281 
283  ptu_if_->msgq_enqueue(gm);
284  }
285  }
286  }
287 
288  void
289  exec_move()
290  {
291  init_bb();
292 
293  float pan, tilt, new_pan, new_tilt;
294 
295  ptu_if_->read();
296  pan = new_pan = ptu_if_->pan();
297  tilt = new_tilt = ptu_if_->tilt();
298 
299  const std::vector<const char *> &items = argp_.items();
300  for (unsigned int i = 1; i < items.size(); ++i) {
301  if (strcmp(items[i], "pan") == 0) {
302  if (items.size() > i + 1) {
303  new_pan = argp_.parse_item_float(++i);
304  } else {
305  printf("No pan value supplied, aborting.\n");
306  return;
307  }
308  } else if (strcmp(items[i], "tilt") == 0) {
309  if (items.size() > i + 1) {
310  new_tilt = argp_.parse_item_float(++i);
311  } else {
312  printf("No tilt value supplied, aborting.\n");
313  return;
314  }
315  } else {
316  printf("Unknown parameter '%s', aborting.\n", items[i]);
317  return;
318  }
319  }
320 
321  if ((pan != new_pan) || (tilt != new_tilt)) {
322  printf("Goto pan %f and tilt %f\n", new_pan, new_tilt);
323 
325  new PanTiltInterface::SetVelocityMessage(ptu_if_->max_pan_velocity() / 2.,
326  ptu_if_->max_tilt_velocity() / 2.);
327  ptu_if_->msgq_enqueue(svm);
328 
330  ptu_if_->msgq_enqueue(gm);
331  usleep(5e5);
332  }
333  }
334 
335  void
336  rx28_set_id()
337  {
338  init_rx28();
339 
340  int old_id = argp_.parse_item_int(1);
341  int new_id = argp_.parse_item_int(2);
342 
343  printf("Servo IDs *before* setting the ID:\n");
344  RobotisRX28::DeviceList devl = rx28_->discover();
345  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
346  printf(" %d\n", *i);
347  }
348 
349  if (old_id < 0 || old_id >= RobotisRX28::BROADCAST_ID) {
350  printf("Invalid old ID %i, must be in range [0..%u]\n", old_id, RobotisRX28::BROADCAST_ID);
351  return;
352  }
353 
354  if (new_id < 0 || new_id >= RobotisRX28::BROADCAST_ID) {
355  printf("Invalid new ID %i, must be in range [0..%u]\n", new_id, RobotisRX28::BROADCAST_ID);
356  return;
357  }
358 
359  rx28_->set_id(old_id, new_id);
360 
361  printf("Servo IDs *after* setting the ID:\n");
362  devl = rx28_->discover();
363  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
364  printf(" %d\n", *i);
365  }
366  }
367 
368  void
369  rx28_discover()
370  {
371  init_rx28();
372 
373  printf("Servo IDs on the bus:\n");
374  RobotisRX28::DeviceList devl = rx28_->discover();
375  for (RobotisRX28::DeviceList::iterator i = devl.begin(); i != devl.end(); ++i) {
376  printf(" %d\n", *i);
377  }
378  }
379 
380 private:
381  ArgumentParser argp_;
382  BlackBoard * bb_;
383  PanTiltInterface *ptu_if_;
384  float resolution_;
385  RobotisRX28 * rx28_;
386 };
387 
388 /** Config tool main.
389  * @param argc argument count
390  * @param argv arguments
391  */
392 int
393 main(int argc, char **argv)
394 {
395  try {
396  PTUJoystickControl ptuctrl(argc, argv);
397  ptuctrl.run();
398  } catch (Exception &e) {
399  printf("Running failed: %s\n\n", e.what());
400  print_usage(argv[0]);
401  exit(0);
402  }
403 
404  return 0;
405 }
PTUJoystickControl::~PTUJoystickControl
~PTUJoystickControl()
Destructor.
Definition: ffptu.cpp:88
RobotisRX28::DeviceList
std::list< unsigned char > DeviceList
List of servo IDs.
Definition: rx28.h:47
fawkes::PanTiltInterface
PanTiltInterface Fawkes BlackBoard Interface.
Definition: PanTiltInterface.h:34
fawkes::BlackBoard
The BlackBoard abstract class.
Definition: blackboard.h:46
fawkes::RemoteBlackBoard
Remote BlackBoard.
Definition: remote.h:49
fawkes::PanTiltInterface::SetVelocityMessage
SetVelocityMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:252
PTUJoystickControl::init_rx28
void init_rx28()
Initialize Robotis RX28 raw servo access.
Definition: ffptu.cpp:149
fawkes::PanTiltInterface::GotoMessage
GotoMessage Fawkes BlackBoard Interface Message.
Definition: PanTiltInterface.h:164
fawkes
Fawkes library namespace.
fawkes::ArgumentParser
Parse command line arguments.
Definition: argparser.h:64
PTUJoystickControl
Remote control PTUs via keyboard.
Definition: ffptu.cpp:68
fawkes::InterfaceInfoList
Interface information list.
Definition: interface_info.h:76
RobotisRX28
Class to access a chain of Robotis RX28 servos.
Definition: rx28.h:44
RobotisRX28::BROADCAST_ID
static const unsigned char BROADCAST_ID
BROADCAST_ID.
Definition: rx28.h:147
PTUJoystickControl::run
void run()
Run control loop.
Definition: ffptu.cpp:161
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
fawkes::getkey
char getkey(int timeout_decisecs)
Get value of a single key-press non-blocking.
Definition: getkey.cpp:68
PTUJoystickControl::PTUJoystickControl
PTUJoystickControl(int argc, char **argv)
Constructor.
Definition: ffptu.cpp:74
PTUJoystickControl::init_bb
void init_bb()
Initialize BB connection.
Definition: ffptu.cpp:99
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36