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 <sys/ioctl.h>
28 #include <sys/stat.h>
29 #include <sys/time.h>
30 #include <sys/types.h>
31 #include <utils/math/angle.h>
32 
33 #include <cerrno>
34 #include <cstdio>
35 #include <cstdlib>
36 #include <cstring>
37 #include <fcntl.h>
38 #include <termios.h>
39 #include <unistd.h>
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 /** Constructor.
94  * @param device_file serial device file (e.g. /dev/ttyS0)
95  * @param timeout_ms timeout for read operations in miliseconds
96  */
97 DirectedPerceptionPTU::DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms)
98 {
99  device_file_ = strdup(device_file);
100  opened_ = false;
101  timeout_ms_ = timeout_ms;
102 
103  open();
104 }
105 
106 /** Destructor. */
108 {
109  close();
110  free(device_file_);
111 }
112 
113 void
114 DirectedPerceptionPTU::open()
115 {
116  if (opened_)
117  return;
118 
119  fd_ = ::open(device_file_, O_RDWR | O_NOCTTY | O_NONBLOCK);
120  if (!fd_ || !isatty(fd_)) {
121  throw Exception("Cannot open device or device is not a TTY");
122  }
123 
124  struct termios param;
125 
126  if (tcgetattr(fd_, &param) != 0) {
127  ::close(fd_);
128  throw Exception("DP PTU: Cannot get parameters");
129  ;
130  }
131 
132  if (cfsetspeed(&param, B9600) == -1) {
133  ::close(fd_);
134  throw Exception("DP PTU: Cannot set speed");
135  ;
136  }
137 
138  cfsetospeed(&param, B9600);
139  cfsetispeed(&param, B9600);
140 
141  // set serial line options
142  param.c_cflag |= (CLOCAL | CREAD); // set to local and enable the receiver
143  param.c_cflag &= ~CSIZE; // mask character size bits
144  param.c_cflag |= CS8; // select 8 data bits
145  param.c_cflag &= ~PARENB; // no parity
146  param.c_cflag &= ~CSTOPB; // 1 stop bit
147 
148  // set input options
149  param.c_iflag &= ~(INPCK | ISTRIP); // no input parity checking
150  param.c_iflag &= ~(IXON | IXOFF | IXANY); // no software flow control
151 
152  param.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
153 
154  param.c_cc[VTIME] = 1; // wait for a tenth of a second for data
155  param.c_cc[VMIN] = 0;
156 
157  if (tcsetattr(fd_, TCSANOW, &param) != 0) {
158  ::close(fd_);
159  throw Exception("DP PTU: Cannot set parameters");
160  ;
161  }
162 
163  // get initial values
164  send(DPPTU_RESTORE);
165  send(DPPTU_ECHO_DISABLE);
166  send(DPPTU_ASCII_TERSE);
167 
168  send(DPPTU_RESET);
169 
170  pan_resolution_ = query_int(DPPTU_PAN_RESOLUTION);
171  tilt_resolution_ = query_int(DPPTU_TILT_RESOLUTION);
172 
173  pan_upper_limit_ = query_int(DPPTU_PAN_MAX);
174  pan_lower_limit_ = query_int(DPPTU_PAN_MIN);
175  tilt_upper_limit_ = query_int(DPPTU_TILT_MAX);
176  tilt_lower_limit_ = query_int(DPPTU_TILT_MIN);
177 
178  opened_ = true;
179 }
180 
181 void
182 DirectedPerceptionPTU::close()
183 {
184  if (opened_) {
185  ::close(fd_);
186  opened_ = false;
187  }
188 }
189 
190 /** Stop currently running motion. */
191 void
193 {
194  send(DPPTU_HALT_ALL);
195 }
196 
197 /** Set pan in motor ticks.
198  * @param pan pan position in ticks
199  */
200 void
202 {
203  send(DPPTU_PAN_ABSPOS, pan);
204 }
205 
206 /** Set tilt in motor ticks.
207  * @param tilt tilt position in ticks
208  */
209 void
211 {
212  send(DPPTU_TILT_ABSPOS, tilt);
213 }
214 
215 /** Set pan and tilt in motor ticks.
216  * @param pan pan position in ticks
217  * @param tilt tilt position in ticks
218  */
219 void
221 {
222  if (pan > pan_upper_limit_)
223  pan = pan_upper_limit_;
224  if (pan < pan_lower_limit_)
225  pan = pan_lower_limit_;
226  if (tilt > tilt_upper_limit_)
227  tilt = tilt_upper_limit_;
228  if (tilt < tilt_lower_limit_)
229  tilt = tilt_lower_limit_;
230 
231  send(DPPTU_PAN_ABSPOS, pan);
232  send(DPPTU_TILT_ABSPOS, tilt);
233 }
234 
235 /** Set pan and tilt in radians.
236  * @param pan pan position rad
237  * @param tilt tilt position rad
238  */
239 void
241 {
242  set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
243 }
244 
245 /** Get current position in motor ticks.
246  * @param pan upon return contains current pan position in motor ticks
247  * @param tilt upon return contains current tilt position in motor ticks
248  */
249 void
251 {
252  pan = query_int(DPPTU_PAN_ABSPOS);
253  tilt = query_int(DPPTU_TILT_ABSPOS);
254 }
255 
256 /** Get pan/tilt in radians.
257  * @param pan upon return contains current pan position in radians
258  * @param tilt upon return contains current tilt position in radians
259  */
260 void
262 {
263  int tpan = 0, ttilt = 0;
264 
265  tpan = query_int(DPPTU_PAN_ABSPOS);
266  ttilt = query_int(DPPTU_TILT_ABSPOS);
267 
268  pan = pan_ticks2rad(tpan);
269  tilt = tilt_ticks2rad(ttilt);
270 }
271 
272 /** Get current pan in motor ticks.
273  * @return current pan in motor ticks
274  */
275 int
277 {
278  return query_int(DPPTU_PAN_ABSPOS);
279 }
280 
281 /** Get current tilt in motor ticks.
282  * @return current tilt in motor ticks
283  */
284 int
286 {
287  return query_int(DPPTU_TILT_ABSPOS);
288 }
289 
290 /** Get maximum pan in motor ticks.
291  * @return maximum pan in motor ticks
292  */
293 int
295 {
296  return pan_upper_limit_;
297 }
298 
299 /** Get minimum pan in motor ticks.
300  * @return minimum pan in motor ticks
301  */
302 int
304 {
305  return pan_lower_limit_;
306 }
307 
308 /** Get maximum tilt in motor ticks.
309  * @return maximum tilt in motor ticks
310  */
311 int
313 {
314  return tilt_upper_limit_;
315 }
316 
317 /** Get minimum tilt in motor ticks.
318  * @return minimum tilt in motor ticks
319  */
320 int
322 {
323  return tilt_lower_limit_;
324 }
325 
326 /** Get position limits in radians.
327  * @param pan_min upon return contains minimum pan in radians
328  * @param pan_max upon return contains maximum pan in radians
329  * @param tilt_min upon return contains minimum tilt in radians
330  * @param tilt_max upon return contains maximum tilt in radians
331  */
332 void
333 DirectedPerceptionPTU::get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
334 {
335  pan_min = pan_ticks2rad(pan_lower_limit_);
336  pan_max = pan_ticks2rad(tilt_upper_limit_);
337  tilt_min = tilt_ticks2rad(tilt_lower_limit_);
338  tilt_max = tilt_ticks2rad(tilt_upper_limit_);
339 }
340 
341 /** Reset the PTU. */
342 void
344 {
345  send(DPPTU_RESET);
346 }
347 
348 void
349 DirectedPerceptionPTU::send(const char *command, int value)
350 {
351  snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE, "%s%i ", command, value);
352  write(obuffer_);
353  if (!result_ok()) {
354  printf("Writing with value '%s' to PTU failed\n", obuffer_);
355  }
356 }
357 
358 void
359 DirectedPerceptionPTU::send(const char *command)
360 {
361  snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE, "%s ", command);
362  write(obuffer_);
363  if (!result_ok()) {
364  printf("Writing '%s' to PTU failed\n", obuffer_);
365  }
366 }
367 
368 void
369 DirectedPerceptionPTU::write(const char *buffer)
370 {
371  printf("Writing '%s'\n", obuffer_);
372 
373  tcflush(fd_, TCIOFLUSH);
374  unsigned int buffer_size = strlen(buffer);
375  int written = ::write(fd_, buffer, buffer_size);
376  tcdrain(fd_);
377 
378  if (written < 0) {
379  printf("Writing '%s' failed: %s\n", buffer, strerror(errno));
380  } else if ((unsigned int)written != buffer_size) {
381  printf("Writing '%s' failed, only wrote %i of %u bytes\n", buffer, written, buffer_size);
382  }
383 }
384 
385 bool
386 DirectedPerceptionPTU::read(char *buffer, unsigned int buffer_size)
387 {
388  // wait for message
389  timeval start, now;
390  unsigned int diff_msec = 0;
391  gettimeofday(&start, NULL);
392 
393  int num_bytes = 0;
394  ioctl(fd_, FIONREAD, &num_bytes);
395  while (((timeout_ms_ == 0) || (diff_msec < timeout_ms_)) && (num_bytes == 0)) {
396  ioctl(fd_, FIONREAD, &num_bytes);
397 
398  gettimeofday(&now, NULL);
399  diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
400  usleep(timeout_ms_ * 100);
401  }
402  if (num_bytes == 0) {
403  return false;
404  }
405  ssize_t bytes_read = ::read(fd_, buffer, buffer_size);
406  if (bytes_read < 0) {
407  return false;
408  } else {
409  return (bytes_read > 0);
410  }
411 }
412 
413 bool
414 DirectedPerceptionPTU::result_ok()
415 {
416  if (read(ibuffer_, 1)) {
417  if (ibuffer_[0] == '*') {
418  return true;
419  }
420  }
421 
422  return false;
423 }
424 
425 bool
426 DirectedPerceptionPTU::data_available()
427 {
428  int num_bytes = 0;
429  ioctl(fd_, FIONREAD, &num_bytes);
430  return (num_bytes > 0);
431 }
432 
433 int
434 DirectedPerceptionPTU::query_int(const char *query_command)
435 {
436  send(query_command);
437  bool ok = read(ibuffer_, DPPTU_MAX_IBUFFER_SIZE);
438  if (!ok) {
439  throw Exception("DP PTU: failed to query integer");
440  }
441  int intrv = 0;
442  if (sscanf(ibuffer_, "* %i", &intrv) <= 0) {
443  throw Exception(errno, "DP PTU: failed to query int");
444  }
445  return intrv;
446 }
447 
448 int
449 DirectedPerceptionPTU::pan_rad2ticks(float r)
450 {
451  if (pan_resolution_ == 0)
452  return 0;
453  return (int)rint(rad2deg(r) * 3600 / pan_resolution_);
454 }
455 
456 int
457 DirectedPerceptionPTU::tilt_rad2ticks(float r)
458 {
459  if (tilt_resolution_ == 0)
460  return 0;
461  return (int)rint(rad2deg(r) * 3600 / tilt_resolution_);
462 }
463 
464 float
465 DirectedPerceptionPTU::pan_ticks2rad(int ticks)
466 {
467  if (pan_resolution_ == 0)
468  return 0;
469  return deg2rad(ticks * pan_resolution_ / 3600);
470 }
471 
472 float
473 DirectedPerceptionPTU::tilt_ticks2rad(int ticks)
474 {
475  if (tilt_resolution_ == 0)
476  return 0;
477  return deg2rad(ticks * tilt_resolution_ / 3600);
478 }
DirectedPerceptionPTU::min_tilt
virtual int min_tilt()
Get minimum tilt in motor ticks.
Definition: dp_ptu.cpp:321
DirectedPerceptionPTU::get_pan
virtual int get_pan()
Get current pan in motor ticks.
Definition: dp_ptu.cpp:276
DirectedPerceptionPTU::set_pan_tilt_rad
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
Definition: dp_ptu.cpp:240
DirectedPerceptionPTU::stop_motion
virtual void stop_motion()
Stop currently running motion.
Definition: dp_ptu.cpp:192
DirectedPerceptionPTU::get_pan_tilt_rad
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
Definition: dp_ptu.cpp:261
DirectedPerceptionPTU::set_pan_tilt
virtual void set_pan_tilt(int pan, int tilt)
Set pan and tilt in motor ticks.
Definition: dp_ptu.cpp:220
DirectedPerceptionPTU::get_pan_tilt
virtual void get_pan_tilt(int &pan, int &tilt)
Get current position in motor ticks.
Definition: dp_ptu.cpp:250
fawkes::rad2deg
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:52
DirectedPerceptionPTU::max_tilt
virtual int max_tilt()
Get maximum tilt in motor ticks.
Definition: dp_ptu.cpp:312
DirectedPerceptionPTU::~DirectedPerceptionPTU
virtual ~DirectedPerceptionPTU()
Destructor.
Definition: dp_ptu.cpp:107
DirectedPerceptionPTU::set_pan
virtual void set_pan(int pan)
Set pan in motor ticks.
Definition: dp_ptu.cpp:201
DirectedPerceptionPTU::get_tilt
virtual int get_tilt()
Get current tilt in motor ticks.
Definition: dp_ptu.cpp:285
fawkes
DirectedPerceptionPTU::set_tilt
virtual void set_tilt(int tilt)
Set tilt in motor ticks.
Definition: dp_ptu.cpp:210
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
DirectedPerceptionPTU::reset
virtual void reset()
Reset the PTU.
Definition: dp_ptu.cpp:343
DirectedPerceptionPTU::get_limits
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:333
DirectedPerceptionPTU::min_pan
virtual int min_pan()
Get minimum pan in motor ticks.
Definition: dp_ptu.cpp:303
DirectedPerceptionPTU::DirectedPerceptionPTU
DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms=10)
Constructor.
Definition: dp_ptu.cpp:97
fawkes::Exception
Definition: exception.h:41
DirectedPerceptionPTU::max_pan
virtual int max_pan()
Get maximum pan in motor ticks.
Definition: dp_ptu.cpp:294