26 #include <core/exceptions/system.h>
27 #include <utils/math/angle.h>
29 #include <sys/types.h>
33 #include <sys/ioctl.h>
42 using namespace fawkes;
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";
99 unsigned int timeout_ms)
101 __device_file = strdup(device_file);
103 __timeout_ms = timeout_ms;
119 DirectedPerceptionPTU::open()
121 if (__opened)
return;
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");
128 struct termios param;
130 if (tcgetattr(__fd, ¶m) != 0) {
132 throw Exception(
"DP PTU: Cannot get parameters");;
135 if ( cfsetspeed( ¶m, B9600 ) == -1 ) {
137 throw Exception(
"DP PTU: Cannot set speed");;
140 cfsetospeed(¶m, B9600);
141 cfsetispeed(¶m, B9600);
144 param.c_cflag |= ( CLOCAL | CREAD );
145 param.c_cflag &= ~CSIZE;
146 param.c_cflag |= CS8;
147 param.c_cflag &= ~PARENB;
148 param.c_cflag &= ~CSTOPB;
151 param.c_iflag &= ~( INPCK | ISTRIP );
152 param.c_iflag &= ~( IXON | IXOFF | IXANY );
154 param.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG );
156 param.c_cc[ VTIME ] = 1;
157 param.c_cc[ VMIN ] = 0;
159 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) {
161 throw Exception(
"DP PTU: Cannot set parameters");;
166 send(DPPTU_ECHO_DISABLE);
167 send(DPPTU_ASCII_TERSE);
171 __pan_resolution = query_int(DPPTU_PAN_RESOLUTION);
172 __tilt_resolution = query_int(DPPTU_TILT_RESOLUTION);
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);
184 DirectedPerceptionPTU::close()
197 send(DPPTU_HALT_ALL);
207 send(DPPTU_PAN_ABSPOS, pan);
217 send(DPPTU_TILT_ABSPOS, tilt);
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;
233 send(DPPTU_PAN_ABSPOS, pan);
234 send(DPPTU_TILT_ABSPOS, tilt);
245 set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
256 pan = query_int(DPPTU_PAN_ABSPOS);
257 tilt = query_int(DPPTU_TILT_ABSPOS);
268 int tpan = 0, ttilt = 0;
270 tpan = query_int(DPPTU_PAN_ABSPOS);
271 ttilt = query_int(DPPTU_TILT_ABSPOS);
273 pan = pan_ticks2rad(tpan);
274 tilt = tilt_ticks2rad(ttilt);
284 return query_int(DPPTU_PAN_ABSPOS);
294 return query_int(DPPTU_TILT_ABSPOS);
303 return __pan_upper_limit;
313 return __pan_lower_limit;
324 return __tilt_upper_limit;
334 return __tilt_lower_limit;
346 float &tilt_min,
float &tilt_max)
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);
364 DirectedPerceptionPTU::send(
const char *command,
int value)
366 snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE,
"%s%i ", command, value);
368 if ( ! result_ok() ) {
369 printf(
"Writing with value '%s' to PTU failed\n", __obuffer);
375 DirectedPerceptionPTU::send(
const char *command)
377 snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE,
"%s ", command);
379 if ( ! result_ok() ) {
380 printf(
"Writing '%s' to PTU failed\n", __obuffer);
386 DirectedPerceptionPTU::write(
const char *buffer)
388 printf(
"Writing '%s'\n", __obuffer);
390 tcflush( __fd, TCIOFLUSH );
391 unsigned int buffer_size = strlen(buffer);
392 int written = ::write(__fd, buffer, buffer_size);
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);
404 DirectedPerceptionPTU::read(
char *buffer,
unsigned int buffer_size)
408 unsigned int diff_msec = 0;
409 gettimeofday(&start, NULL);
412 ioctl(__fd, FIONREAD, &num_bytes);
413 while ( ((__timeout_ms == 0) || (diff_msec < __timeout_ms)) && (num_bytes == 0)) {
414 ioctl(__fd, FIONREAD, &num_bytes);
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);
420 if (num_bytes == 0) {
423 int bytes_read = ::read(__fd, buffer, buffer_size);
424 if ( bytes_read < 0 ) {
427 if ((
unsigned int)bytes_read == buffer_size) {
437 DirectedPerceptionPTU::result_ok()
439 if ( read(__ibuffer, 1) ) {
440 if ( __ibuffer[0] ==
'*' ) {
450 DirectedPerceptionPTU::data_available()
453 ioctl(__fd, FIONREAD, &num_bytes);
454 return (num_bytes > 0);
459 DirectedPerceptionPTU::query_int(
const char *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) {
469 sscanf(__ibuffer,
"* %i", &rv);
475 DirectedPerceptionPTU::pan_rad2ticks(
float r)
477 if ( __pan_resolution == 0 )
return 0;
478 return (
int)rint(
rad2deg(r) * 3600 / __pan_resolution);
483 DirectedPerceptionPTU::tilt_rad2ticks(
float r)
485 if ( __tilt_resolution == 0 )
return 0;
486 return (
int)rint(
rad2deg(r) * 3600 / __tilt_resolution);
491 DirectedPerceptionPTU::pan_ticks2rad(
int ticks)
493 if ( __pan_resolution == 0 )
return 0;
494 return deg2rad(ticks * __pan_resolution / 3600);
499 DirectedPerceptionPTU::tilt_ticks2rad(
int ticks)
501 if ( __tilt_resolution == 0 )
return 0;
502 return deg2rad(ticks * __tilt_resolution / 3600);