Fawkes API  Fawkes Development Version
dp_ptu.cpp
1 
2 /***************************************************************************
3  * dp_ptu.cpp - Controller for Directed Perception, Inc. Pan-Tilt Unit on B21
4  *
5  * Created: Wed Nov 29 23:05:49 2006
6  * Copyright 2005-2009 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. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
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_WRE file in the doc directory.
22  */
23 
24 #include "dp_ptu.h"
25 
26 #include <core/exceptions/system.h>
27 #include <utils/math/angle.h>
28 
29 #include <sys/types.h>
30 #include <sys/stat.h>
31 #include <fcntl.h>
32 #include <unistd.h>
33 #include <sys/ioctl.h>
34 #include <sys/time.h>
35 #include <termios.h>
36 #include <cstdio>
37 #include <cstdlib>
38 #include <cstring>
39 #include <cerrno>
40 
41 using namespace std;
42 using namespace fawkes;
43 
44 /** @class DirectedPerceptionPTU "dp_ptu.h"
45  * DirectedPerception PTU implementation.
46  * Control object to use the DirectedPerception PTU Pan/Tilt unit mounted
47  * on carl.
48  *
49  * @author Tim Niemueller
50  */
51 
52 const char * DirectedPerceptionPTU::DPPTU_PAN_ABSPOS = "PP";
53 const char * DirectedPerceptionPTU::DPPTU_TILT_ABSPOS = "TP";
54 const char * DirectedPerceptionPTU::DPPTU_PAN_RELPOS = "PO";
55 const char * DirectedPerceptionPTU::DPPTU_TILT_RELPOS = "TO";
56 const char * DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION = "PR";
57 const char * DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION = "TR";
58 const char * DirectedPerceptionPTU::DPPTU_PAN_MIN = "PN";
59 const char * DirectedPerceptionPTU::DPPTU_PAN_MAX = "PX";
60 const char * DirectedPerceptionPTU::DPPTU_TILT_MIN = "TN";
61 const char * DirectedPerceptionPTU::DPPTU_TILT_MAX = "TX";
62 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY = "L";
63 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE = "LE";
64 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE = "LD";
65 const char * DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION = "I";
66 const char * DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION = "S";
67 const char * DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION = "A";
68 const char * DirectedPerceptionPTU::DPPTU_HALT_ALL = "H";
69 const char * DirectedPerceptionPTU::DPPTU_HALT_PAN = "HP";
70 const char * DirectedPerceptionPTU::DPPTU_HALT_TILT = "HT";
71 const char * DirectedPerceptionPTU::DPPTU_PAN_SPEED = "PS";
72 const char * DirectedPerceptionPTU::DPPTU_TILT_SPEED = "TS";
73 const char * DirectedPerceptionPTU::DPPTU_PAN_ACCEL = "PA";
74 const char * DirectedPerceptionPTU::DPPTU_TILT_ACCEL = "TA";
75 const char * DirectedPerceptionPTU::DPPTU_PAN_BASESPEED = "PB";
76 const char * DirectedPerceptionPTU::DPPTU_TILT_BASESPEED = "TB";
77 const char * DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT = "PU";
78 const char * DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT = "PL";
79 const char * DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT = "TU";
80 const char * DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT = "TL";
81 const char * DirectedPerceptionPTU::DPPTU_RESET = "R";
82 const char * DirectedPerceptionPTU::DPPTU_STORE = "DS";
83 const char * DirectedPerceptionPTU::DPPTU_RESTORE = "DR";
84 const char * DirectedPerceptionPTU::DPPTU_FACTORY_RESET = "DF";
85 const char * DirectedPerceptionPTU::DPPTU_ECHO_QUERY = "E";
86 const char * DirectedPerceptionPTU::DPPTU_ECHO_ENABLE = "EE";
87 const char * DirectedPerceptionPTU::DPPTU_ECHO_DISABLE = "ED";
88 const char * DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE = "FV";
89 const char * DirectedPerceptionPTU::DPPTU_ASCII_TERSE = "FT";
90 const char * DirectedPerceptionPTU::DPPTU_ASCII_QUERY = "F";
91 const char * DirectedPerceptionPTU::DPPTU_VERSION = "V";
92 
93 
94 /** Constructor.
95  * @param device_file serial device file (e.g. /dev/ttyS0)
96  * @param timeout_ms timeout for read operations in miliseconds
97  */
99  unsigned int timeout_ms)
100 {
101  __device_file = strdup(device_file);
102  __opened = false;
103  __timeout_ms = timeout_ms;
104 
105  open();
106 }
107 
108 
109 
110 /** Destructor. */
112 {
113  close();
114  free(__device_file);
115 }
116 
117 
118 void
119 DirectedPerceptionPTU::open()
120 {
121  if (__opened) return;
122 
123  __fd = ::open(__device_file, O_RDWR | O_NOCTTY | O_NONBLOCK);
124  if ( ! __fd || ! isatty(__fd)) {
125  throw Exception("Cannot open device or device is not a TTY");
126  }
127 
128  struct termios param;
129 
130  if (tcgetattr(__fd, &param) != 0) {
131  ::close(__fd);
132  throw Exception("DP PTU: Cannot get parameters");;
133  }
134 
135  if ( cfsetspeed( &param, B9600 ) == -1 ) {
136  ::close( __fd );
137  throw Exception("DP PTU: Cannot set speed");;
138  }
139 
140  cfsetospeed(&param, B9600);
141  cfsetispeed(&param, B9600);
142 
143  // set serial line options
144  param.c_cflag |= ( CLOCAL | CREAD ); // set to local and enable the receiver
145  param.c_cflag &= ~CSIZE; // mask character size bits
146  param.c_cflag |= CS8; // select 8 data bits
147  param.c_cflag &= ~PARENB; // no parity
148  param.c_cflag &= ~CSTOPB; // 1 stop bit
149 
150  // set input options
151  param.c_iflag &= ~( INPCK | ISTRIP ); // no input parity checking
152  param.c_iflag &= ~( IXON | IXOFF | IXANY ); // no software flow control
153 
154  param.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG );
155 
156  param.c_cc[ VTIME ] = 1; // wait for a tenth of a second for data
157  param.c_cc[ VMIN ] = 0;
158 
159  if (tcsetattr(__fd, TCSANOW, &param) != 0) {
160  ::close(__fd);
161  throw Exception("DP PTU: Cannot set parameters");;
162  }
163 
164  // get initial values
165  send(DPPTU_RESTORE);
166  send(DPPTU_ECHO_DISABLE);
167  send(DPPTU_ASCII_TERSE);
168 
169  send(DPPTU_RESET);
170 
171  __pan_resolution = query_int(DPPTU_PAN_RESOLUTION);
172  __tilt_resolution = query_int(DPPTU_TILT_RESOLUTION);
173 
174  __pan_upper_limit = query_int(DPPTU_PAN_MAX);
175  __pan_lower_limit = query_int(DPPTU_PAN_MIN);
176  __tilt_upper_limit = query_int(DPPTU_TILT_MAX);
177  __tilt_lower_limit = query_int(DPPTU_TILT_MIN);
178 
179  __opened = true;
180 }
181 
182 
183 void
184 DirectedPerceptionPTU::close()
185 {
186  if (__opened) {
187  ::close(__fd);
188  __opened = false;
189  }
190 }
191 
192 
193 /** Stop currently running motion. */
194 void
196 {
197  send(DPPTU_HALT_ALL);
198 }
199 
200 
201 /** Set pan in motor ticks.
202  * @param pan pan position in ticks
203  */
204 void
206 {
207  send(DPPTU_PAN_ABSPOS, pan);
208 }
209 
210 
211 /** Set tilt in motor ticks.
212  * @param tilt tilt position in ticks
213  */
214 void
216 {
217  send(DPPTU_TILT_ABSPOS, tilt);
218 }
219 
220 
221 /** Set pan and tilt in motor ticks.
222  * @param pan pan position in ticks
223  * @param tilt tilt position in ticks
224  */
225 void
227 {
228  if ( pan > __pan_upper_limit ) pan = __pan_upper_limit;
229  if ( pan < __pan_lower_limit ) pan = __pan_lower_limit;
230  if ( tilt > __tilt_upper_limit ) tilt = __tilt_upper_limit;
231  if ( tilt < __tilt_lower_limit ) tilt = __tilt_lower_limit;
232 
233  send(DPPTU_PAN_ABSPOS, pan);
234  send(DPPTU_TILT_ABSPOS, tilt);
235 }
236 
237 
238 /** Set pan and tilt in radians.
239  * @param pan pan position rad
240  * @param tilt tilt position rad
241  */
242 void
244 {
245  set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
246 }
247 
248 
249 /** Get current position in motor ticks.
250  * @param pan upon return contains current pan position in motor ticks
251  * @param tilt upon return contains current tilt position in motor ticks
252  */
253 void
255 {
256  pan = query_int(DPPTU_PAN_ABSPOS);
257  tilt = query_int(DPPTU_TILT_ABSPOS);
258 }
259 
260 
261 /** Get pan/tilt in radians.
262  * @param pan upon return contains current pan position in radians
263  * @param tilt upon return contains current tilt position in radians
264  */
265 void
267 {
268  int tpan = 0, ttilt = 0;
269 
270  tpan = query_int(DPPTU_PAN_ABSPOS);
271  ttilt = query_int(DPPTU_TILT_ABSPOS);
272 
273  pan = pan_ticks2rad(tpan);
274  tilt = tilt_ticks2rad(ttilt);
275 }
276 
277 
278 /** Get current pan in motor ticks.
279  * @return current pan in motor ticks
280  */
281 int
283 {
284  return query_int(DPPTU_PAN_ABSPOS);
285 }
286 
287 
288 /** Get current tilt in motor ticks.
289  * @return current tilt in motor ticks
290  */
291 int
293 {
294  return query_int(DPPTU_TILT_ABSPOS);
295 }
296 
297 /** Get maximum pan in motor ticks.
298  * @return maximum pan in motor ticks
299  */
300 int
302 {
303  return __pan_upper_limit;
304 }
305 
306 
307 /** Get minimum pan in motor ticks.
308  * @return minimum pan in motor ticks
309  */
310 int
312 {
313  return __pan_lower_limit;
314 
315 }
316 
317 
318 /** Get maximum tilt in motor ticks.
319  * @return maximum tilt in motor ticks
320  */
321 int
323 {
324  return __tilt_upper_limit;
325 }
326 
327 
328 /** Get minimum tilt in motor ticks.
329  * @return minimum tilt in motor ticks
330  */
331 int
333 {
334  return __tilt_lower_limit;
335 }
336 
337 
338 /** Get position limits in radians.
339  * @param pan_min upon return contains minimum pan in radians
340  * @param pan_max upon return contains maximum pan in radians
341  * @param tilt_min upon return contains minimum tilt in radians
342  * @param tilt_max upon return contains maximum tilt in radians
343  */
344 void
345 DirectedPerceptionPTU::get_limits(float &pan_min, float &pan_max,
346  float &tilt_min, float &tilt_max)
347 {
348  pan_min = pan_ticks2rad(__pan_lower_limit);
349  pan_max = pan_ticks2rad(__tilt_upper_limit);
350  tilt_min = tilt_ticks2rad(__tilt_lower_limit);
351  tilt_max = tilt_ticks2rad(__tilt_upper_limit);
352 }
353 
354 
355 /** Reset the PTU. */
356 void
358 {
359  send(DPPTU_RESET);
360 }
361 
362 
363 void
364 DirectedPerceptionPTU::send(const char *command, int value)
365 {
366  snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s%i ", command, value);
367  write(__obuffer);
368  if ( ! result_ok() ) {
369  printf("Writing with value '%s' to PTU failed\n", __obuffer);
370  }
371 }
372 
373 
374 void
375 DirectedPerceptionPTU::send(const char *command)
376 {
377  snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s ", command);
378  write(__obuffer);
379  if ( ! result_ok() ) {
380  printf("Writing '%s' to PTU failed\n", __obuffer);
381  }
382 }
383 
384 
385 void
386 DirectedPerceptionPTU::write(const char *buffer)
387 {
388  printf("Writing '%s'\n", __obuffer);
389 
390  tcflush( __fd, TCIOFLUSH );
391  unsigned int buffer_size = strlen(buffer);
392  int written = ::write(__fd, buffer, buffer_size);
393  tcdrain(__fd);
394 
395  if (written < 0) {
396  printf("Writing '%s' failed: %s\n", buffer, strerror(errno));
397  } else if ((unsigned int)written != buffer_size) {
398  printf("Writing '%s' failed, only wrote %i of %u bytes\n", buffer, written, buffer_size);
399  }
400 }
401 
402 
403 bool
404 DirectedPerceptionPTU::read(char *buffer, unsigned int buffer_size)
405 {
406  // wait for message
407  timeval start, now;
408  unsigned int diff_msec = 0;
409  gettimeofday(&start, NULL);
410 
411  int num_bytes = 0;
412  ioctl(__fd, FIONREAD, &num_bytes);
413  while ( ((__timeout_ms == 0) || (diff_msec < __timeout_ms)) && (num_bytes == 0)) {
414  ioctl(__fd, FIONREAD, &num_bytes);
415 
416  gettimeofday(&now, NULL);
417  diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
418  usleep(__timeout_ms * 100);
419  }
420  if (num_bytes == 0) {
421  return false;
422  }
423  int bytes_read = ::read(__fd, buffer, buffer_size);
424  if ( bytes_read < 0 ) {
425  return false;
426  } else {
427  if ((unsigned int)bytes_read == buffer_size) {
428  return true;
429  } else {
430  return false;
431  }
432  }
433 }
434 
435 
436 bool
437 DirectedPerceptionPTU::result_ok()
438 {
439  if ( read(__ibuffer, 1) ) {
440  if ( __ibuffer[0] == '*' ) {
441  return true;
442  }
443  }
444 
445  return false;
446 }
447 
448 
449 bool
450 DirectedPerceptionPTU::data_available()
451 {
452  int num_bytes = 0;
453  ioctl(__fd, FIONREAD, &num_bytes);
454  return (num_bytes > 0);
455 }
456 
457 
458 int
459 DirectedPerceptionPTU::query_int(const char *query_command)
460 {
461  send(query_command);
462  ssize_t read_bytes = read(__ibuffer, DPPTU_MAX_OBUFFER_SIZE);
463  if ( read_bytes == -1 ) {
464  throw FileReadException(__device_file, errno, "Querying integer from PTU failed");
465  } else if (read_bytes == 0) {
466  return 0;
467  }
468  int rv = 0;
469  sscanf(__ibuffer, "* %i", &rv);
470  return rv;
471 }
472 
473 
474 int
475 DirectedPerceptionPTU::pan_rad2ticks(float r)
476 {
477  if ( __pan_resolution == 0 ) return 0;
478  return (int)rint(rad2deg(r) * 3600 / __pan_resolution);
479 }
480 
481 
482 int
483 DirectedPerceptionPTU::tilt_rad2ticks(float r)
484 {
485  if ( __tilt_resolution == 0 ) return 0;
486  return (int)rint(rad2deg(r) * 3600 / __tilt_resolution);
487 }
488 
489 
490 float
491 DirectedPerceptionPTU::pan_ticks2rad(int ticks)
492 {
493  if ( __pan_resolution == 0 ) return 0;
494  return deg2rad(ticks * __pan_resolution / 3600);
495 }
496 
497 
498 float
499 DirectedPerceptionPTU::tilt_ticks2rad(int ticks)
500 {
501  if ( __tilt_resolution == 0 ) return 0;
502  return deg2rad(ticks * __tilt_resolution / 3600);
503 }
virtual void set_pan_tilt(int pan, int tilt)
Set pan and tilt in motor ticks.
Definition: dp_ptu.cpp:226
virtual int get_pan()
Get current pan in motor ticks.
Definition: dp_ptu.cpp:282
File could not be read.
Definition: system.h:61
Fawkes library namespace.
virtual void set_pan(int pan)
Set pan in motor ticks.
Definition: dp_ptu.cpp:205
virtual ~DirectedPerceptionPTU()
Destructor.
Definition: dp_ptu.cpp:111
STL namespace.
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
Definition: dp_ptu.cpp:266
virtual void set_tilt(int tilt)
Set tilt in motor ticks.
Definition: dp_ptu.cpp:215
virtual void get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get position limits in radians.
Definition: dp_ptu.cpp:345
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
Definition: dp_ptu.cpp:243
virtual void stop_motion()
Stop currently running motion.
Definition: dp_ptu.cpp:195
virtual void reset()
Reset the PTU.
Definition: dp_ptu.cpp:357
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual int max_tilt()
Get maximum tilt in motor ticks.
Definition: dp_ptu.cpp:322
virtual void get_pan_tilt(int &pan, int &tilt)
Get current position in motor ticks.
Definition: dp_ptu.cpp:254
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
virtual int min_tilt()
Get minimum tilt in motor ticks.
Definition: dp_ptu.cpp:332
virtual int min_pan()
Get minimum pan in motor ticks.
Definition: dp_ptu.cpp:311
virtual int max_pan()
Get maximum pan in motor ticks.
Definition: dp_ptu.cpp:301
DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms=10)
Constructor.
Definition: dp_ptu.cpp:98
virtual int get_tilt()
Get current tilt in motor ticks.
Definition: dp_ptu.cpp:292