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
41using namespace std;
42using 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
52const char *DirectedPerceptionPTU::DPPTU_PAN_ABSPOS = "PP";
53const char *DirectedPerceptionPTU::DPPTU_TILT_ABSPOS = "TP";
54const char *DirectedPerceptionPTU::DPPTU_PAN_RELPOS = "PO";
55const char *DirectedPerceptionPTU::DPPTU_TILT_RELPOS = "TO";
56const char *DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION = "PR";
57const char *DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION = "TR";
58const char *DirectedPerceptionPTU::DPPTU_PAN_MIN = "PN";
59const char *DirectedPerceptionPTU::DPPTU_PAN_MAX = "PX";
60const char *DirectedPerceptionPTU::DPPTU_TILT_MIN = "TN";
61const char *DirectedPerceptionPTU::DPPTU_TILT_MAX = "TX";
62const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY = "L";
63const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE = "LE";
64const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE = "LD";
65const char *DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION = "I";
66const char *DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION = "S";
67const char *DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION = "A";
68const char *DirectedPerceptionPTU::DPPTU_HALT_ALL = "H";
69const char *DirectedPerceptionPTU::DPPTU_HALT_PAN = "HP";
70const char *DirectedPerceptionPTU::DPPTU_HALT_TILT = "HT";
71const char *DirectedPerceptionPTU::DPPTU_PAN_SPEED = "PS";
72const char *DirectedPerceptionPTU::DPPTU_TILT_SPEED = "TS";
73const char *DirectedPerceptionPTU::DPPTU_PAN_ACCEL = "PA";
74const char *DirectedPerceptionPTU::DPPTU_TILT_ACCEL = "TA";
75const char *DirectedPerceptionPTU::DPPTU_PAN_BASESPEED = "PB";
76const char *DirectedPerceptionPTU::DPPTU_TILT_BASESPEED = "TB";
77const char *DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT = "PU";
78const char *DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT = "PL";
79const char *DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT = "TU";
80const char *DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT = "TL";
81const char *DirectedPerceptionPTU::DPPTU_RESET = "R";
82const char *DirectedPerceptionPTU::DPPTU_STORE = "DS";
83const char *DirectedPerceptionPTU::DPPTU_RESTORE = "DR";
84const char *DirectedPerceptionPTU::DPPTU_FACTORY_RESET = "DF";
85const char *DirectedPerceptionPTU::DPPTU_ECHO_QUERY = "E";
86const char *DirectedPerceptionPTU::DPPTU_ECHO_ENABLE = "EE";
87const char *DirectedPerceptionPTU::DPPTU_ECHO_DISABLE = "ED";
88const char *DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE = "FV";
89const char *DirectedPerceptionPTU::DPPTU_ASCII_TERSE = "FT";
90const char *DirectedPerceptionPTU::DPPTU_ASCII_QUERY = "F";
91const 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 */
97DirectedPerceptionPTU::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
113void
114DirectedPerceptionPTU::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
181void
182DirectedPerceptionPTU::close()
183{
184 if (opened_) {
185 ::close(fd_);
186 opened_ = false;
187 }
188}
189
190/** Stop currently running motion. */
191void
193{
194 send(DPPTU_HALT_ALL);
195}
196
197/** Set pan in motor ticks.
198 * @param pan pan position in ticks
199 */
200void
202{
203 send(DPPTU_PAN_ABSPOS, pan);
204}
205
206/** Set tilt in motor ticks.
207 * @param tilt tilt position in ticks
208 */
209void
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 */
219void
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 */
239void
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 */
249void
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 */
260void
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 */
275int
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 */
284int
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 */
293int
295{
296 return pan_upper_limit_;
297}
298
299/** Get minimum pan in motor ticks.
300 * @return minimum pan in motor ticks
301 */
302int
304{
305 return pan_lower_limit_;
306}
307
308/** Get maximum tilt in motor ticks.
309 * @return maximum tilt in motor ticks
310 */
311int
313{
314 return tilt_upper_limit_;
315}
316
317/** Get minimum tilt in motor ticks.
318 * @return minimum tilt in motor ticks
319 */
320int
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 */
332void
333DirectedPerceptionPTU::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. */
342void
344{
345 send(DPPTU_RESET);
346}
347
348void
349DirectedPerceptionPTU::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
358void
359DirectedPerceptionPTU::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
368void
369DirectedPerceptionPTU::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
385bool
386DirectedPerceptionPTU::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
413bool
414DirectedPerceptionPTU::result_ok()
415{
416 if (read(ibuffer_, 1)) {
417 if (ibuffer_[0] == '*') {
418 return true;
419 }
420 }
421
422 return false;
423}
424
425bool
426DirectedPerceptionPTU::data_available()
427{
428 int num_bytes = 0;
429 ioctl(fd_, FIONREAD, &num_bytes);
430 return (num_bytes > 0);
431}
432
433int
434DirectedPerceptionPTU::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
448int
449DirectedPerceptionPTU::pan_rad2ticks(float r)
450{
451 if (pan_resolution_ == 0)
452 return 0;
453 return (int)rint(rad2deg(r) * 3600 / pan_resolution_);
454}
455
456int
457DirectedPerceptionPTU::tilt_rad2ticks(float r)
458{
459 if (tilt_resolution_ == 0)
460 return 0;
461 return (int)rint(rad2deg(r) * 3600 / tilt_resolution_);
462}
463
464float
465DirectedPerceptionPTU::pan_ticks2rad(int ticks)
466{
467 if (pan_resolution_ == 0)
468 return 0;
469 return deg2rad(ticks * pan_resolution_ / 3600);
470}
471
472float
473DirectedPerceptionPTU::tilt_ticks2rad(int ticks)
474{
475 if (tilt_resolution_ == 0)
476 return 0;
477 return deg2rad(ticks * tilt_resolution_ / 3600);
478}
virtual int get_tilt()
Get current tilt in motor ticks.
Definition: dp_ptu.cpp:285
DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms=10)
Constructor.
Definition: dp_ptu.cpp:97
virtual void get_pan_tilt(int &pan, int &tilt)
Get current position in motor ticks.
Definition: dp_ptu.cpp:250
virtual int get_pan()
Get current pan in motor ticks.
Definition: dp_ptu.cpp:276
virtual void reset()
Reset the PTU.
Definition: dp_ptu.cpp:343
virtual void set_pan(int pan)
Set pan in motor ticks.
Definition: dp_ptu.cpp:201
virtual void stop_motion()
Stop currently running motion.
Definition: dp_ptu.cpp:192
virtual int max_pan()
Get maximum pan in motor ticks.
Definition: dp_ptu.cpp:294
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
Definition: dp_ptu.cpp:240
virtual ~DirectedPerceptionPTU()
Destructor.
Definition: dp_ptu.cpp:107
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
Definition: dp_ptu.cpp:261
virtual int max_tilt()
Get maximum tilt in motor ticks.
Definition: dp_ptu.cpp:312
virtual int min_pan()
Get minimum pan in motor ticks.
Definition: dp_ptu.cpp:303
virtual int min_tilt()
Get minimum tilt in motor ticks.
Definition: dp_ptu.cpp:321
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
virtual void set_tilt(int tilt)
Set tilt in motor ticks.
Definition: dp_ptu.cpp:210
virtual void set_pan_tilt(int pan, int tilt)
Set pan and tilt in motor ticks.
Definition: dp_ptu.cpp:220
Base class for exceptions in Fawkes.
Definition: exception.h:36
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46