00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "dp_ptu.h"
00025
00026 #include <core/exceptions/system.h>
00027 #include <utils/math/angle.h>
00028
00029 #include <sys/types.h>
00030 #include <sys/stat.h>
00031 #include <fcntl.h>
00032 #include <unistd.h>
00033 #include <sys/ioctl.h>
00034 #include <sys/time.h>
00035 #include <termios.h>
00036 #include <cstdio>
00037 #include <cstdlib>
00038 #include <cstring>
00039 #include <cerrno>
00040
00041 using namespace std;
00042 using namespace fawkes;
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 const char * DirectedPerceptionPTU::DPPTU_PAN_ABSPOS = "PP";
00053 const char * DirectedPerceptionPTU::DPPTU_TILT_ABSPOS = "TP";
00054 const char * DirectedPerceptionPTU::DPPTU_PAN_RELPOS = "PO";
00055 const char * DirectedPerceptionPTU::DPPTU_TILT_RELPOS = "TO";
00056 const char * DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION = "PR";
00057 const char * DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION = "TR";
00058 const char * DirectedPerceptionPTU::DPPTU_PAN_MIN = "PN";
00059 const char * DirectedPerceptionPTU::DPPTU_PAN_MAX = "PX";
00060 const char * DirectedPerceptionPTU::DPPTU_TILT_MIN = "TN";
00061 const char * DirectedPerceptionPTU::DPPTU_TILT_MAX = "TX";
00062 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY = "L";
00063 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE = "LE";
00064 const char * DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE = "LD";
00065 const char * DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION = "I";
00066 const char * DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION = "S";
00067 const char * DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION = "A";
00068 const char * DirectedPerceptionPTU::DPPTU_HALT_ALL = "H";
00069 const char * DirectedPerceptionPTU::DPPTU_HALT_PAN = "HP";
00070 const char * DirectedPerceptionPTU::DPPTU_HALT_TILT = "HT";
00071 const char * DirectedPerceptionPTU::DPPTU_PAN_SPEED = "PS";
00072 const char * DirectedPerceptionPTU::DPPTU_TILT_SPEED = "TS";
00073 const char * DirectedPerceptionPTU::DPPTU_PAN_ACCEL = "PA";
00074 const char * DirectedPerceptionPTU::DPPTU_TILT_ACCEL = "TA";
00075 const char * DirectedPerceptionPTU::DPPTU_PAN_BASESPEED = "PB";
00076 const char * DirectedPerceptionPTU::DPPTU_TILT_BASESPEED = "TB";
00077 const char * DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT = "PU";
00078 const char * DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT = "PL";
00079 const char * DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT = "TU";
00080 const char * DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT = "TL";
00081 const char * DirectedPerceptionPTU::DPPTU_RESET = "R";
00082 const char * DirectedPerceptionPTU::DPPTU_STORE = "DS";
00083 const char * DirectedPerceptionPTU::DPPTU_RESTORE = "DR";
00084 const char * DirectedPerceptionPTU::DPPTU_FACTORY_RESET = "DF";
00085 const char * DirectedPerceptionPTU::DPPTU_ECHO_QUERY = "E";
00086 const char * DirectedPerceptionPTU::DPPTU_ECHO_ENABLE = "EE";
00087 const char * DirectedPerceptionPTU::DPPTU_ECHO_DISABLE = "ED";
00088 const char * DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE = "FV";
00089 const char * DirectedPerceptionPTU::DPPTU_ASCII_TERSE = "FT";
00090 const char * DirectedPerceptionPTU::DPPTU_ASCII_QUERY = "F";
00091 const char * DirectedPerceptionPTU::DPPTU_VERSION = "V";
00092
00093
00094
00095
00096
00097
00098 DirectedPerceptionPTU::DirectedPerceptionPTU(const char *device_file,
00099 unsigned int timeout_ms)
00100 {
00101 __device_file = strdup(device_file);
00102 __opened = false;
00103 __timeout_ms = timeout_ms;
00104
00105 open();
00106 }
00107
00108
00109
00110
00111 DirectedPerceptionPTU::~DirectedPerceptionPTU()
00112 {
00113 close();
00114 free(__device_file);
00115 }
00116
00117
00118 void
00119 DirectedPerceptionPTU::open()
00120 {
00121 if (__opened) return;
00122
00123 __fd = ::open(__device_file, O_RDWR | O_NOCTTY | O_NONBLOCK);
00124 if ( ! __fd || ! isatty(__fd)) {
00125 throw Exception("Cannot open device or device is not a TTY");
00126 }
00127
00128 struct termios param;
00129
00130 if (tcgetattr(__fd, ¶m) != 0) {
00131 ::close(__fd);
00132 throw Exception("DP PTU: Cannot get parameters");;
00133 }
00134
00135 if ( cfsetspeed( ¶m, B9600 ) == -1 ) {
00136 ::close( __fd );
00137 throw Exception("DP PTU: Cannot set speed");;
00138 }
00139
00140 cfsetospeed(¶m, B9600);
00141 cfsetispeed(¶m, B9600);
00142
00143
00144 param.c_cflag |= ( CLOCAL | CREAD );
00145 param.c_cflag &= ~CSIZE;
00146 param.c_cflag |= CS8;
00147 param.c_cflag &= ~PARENB;
00148 param.c_cflag &= ~CSTOPB;
00149
00150
00151 param.c_iflag &= ~( INPCK | ISTRIP );
00152 param.c_iflag &= ~( IXON | IXOFF | IXANY );
00153
00154 param.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG );
00155
00156 param.c_cc[ VTIME ] = 1;
00157 param.c_cc[ VMIN ] = 0;
00158
00159 if (tcsetattr(__fd, TCSANOW, ¶m) != 0) {
00160 ::close(__fd);
00161 throw Exception("DP PTU: Cannot set parameters");;
00162 }
00163
00164
00165 send(DPPTU_RESTORE);
00166 send(DPPTU_ECHO_DISABLE);
00167 send(DPPTU_ASCII_TERSE);
00168
00169 send(DPPTU_RESET);
00170
00171 __pan_resolution = query_int(DPPTU_PAN_RESOLUTION);
00172 __tilt_resolution = query_int(DPPTU_TILT_RESOLUTION);
00173
00174 __pan_upper_limit = query_int(DPPTU_PAN_MAX);
00175 __pan_lower_limit = query_int(DPPTU_PAN_MIN);
00176 __tilt_upper_limit = query_int(DPPTU_TILT_MAX);
00177 __tilt_lower_limit = query_int(DPPTU_TILT_MIN);
00178
00179 __opened = true;
00180 }
00181
00182
00183 void
00184 DirectedPerceptionPTU::close()
00185 {
00186 if (__opened) {
00187 ::close(__fd);
00188 __opened = false;
00189 }
00190 }
00191
00192
00193
00194 void
00195 DirectedPerceptionPTU::stop_motion()
00196 {
00197 send(DPPTU_HALT_ALL);
00198 }
00199
00200
00201
00202
00203
00204 void
00205 DirectedPerceptionPTU::set_pan(int pan)
00206 {
00207 send(DPPTU_PAN_ABSPOS, pan);
00208 }
00209
00210
00211
00212
00213
00214 void
00215 DirectedPerceptionPTU::set_tilt(int tilt)
00216 {
00217 send(DPPTU_TILT_ABSPOS, tilt);
00218 }
00219
00220
00221
00222
00223
00224
00225 void
00226 DirectedPerceptionPTU::set_pan_tilt(int pan, int tilt)
00227 {
00228 if ( pan > __pan_upper_limit ) pan = __pan_upper_limit;
00229 if ( pan < __pan_lower_limit ) pan = __pan_lower_limit;
00230 if ( tilt > __tilt_upper_limit ) tilt = __tilt_upper_limit;
00231 if ( tilt < __tilt_lower_limit ) tilt = __tilt_lower_limit;
00232
00233 send(DPPTU_PAN_ABSPOS, pan);
00234 send(DPPTU_TILT_ABSPOS, tilt);
00235 }
00236
00237
00238
00239
00240
00241
00242 void
00243 DirectedPerceptionPTU::set_pan_tilt_rad(float pan, float tilt)
00244 {
00245 set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
00246 }
00247
00248
00249
00250
00251
00252
00253 void
00254 DirectedPerceptionPTU::get_pan_tilt(int &pan, int &tilt)
00255 {
00256 pan = query_int(DPPTU_PAN_ABSPOS);
00257 tilt = query_int(DPPTU_TILT_ABSPOS);
00258 }
00259
00260
00261
00262
00263
00264
00265 void
00266 DirectedPerceptionPTU::get_pan_tilt_rad(float &pan, float &tilt)
00267 {
00268 int tpan = 0, ttilt = 0;
00269
00270 tpan = query_int(DPPTU_PAN_ABSPOS);
00271 ttilt = query_int(DPPTU_TILT_ABSPOS);
00272
00273 pan = pan_ticks2rad(tpan);
00274 tilt = tilt_ticks2rad(ttilt);
00275 }
00276
00277
00278
00279
00280
00281 int
00282 DirectedPerceptionPTU::get_pan()
00283 {
00284 return query_int(DPPTU_PAN_ABSPOS);
00285 }
00286
00287
00288
00289
00290
00291 int
00292 DirectedPerceptionPTU::get_tilt()
00293 {
00294 return query_int(DPPTU_TILT_ABSPOS);
00295 }
00296
00297
00298
00299
00300 int
00301 DirectedPerceptionPTU::max_pan()
00302 {
00303 return __pan_upper_limit;
00304 }
00305
00306
00307
00308
00309
00310 int
00311 DirectedPerceptionPTU::min_pan()
00312 {
00313 return __pan_lower_limit;
00314
00315 }
00316
00317
00318
00319
00320
00321 int
00322 DirectedPerceptionPTU::max_tilt()
00323 {
00324 return __tilt_upper_limit;
00325 }
00326
00327
00328
00329
00330
00331 int
00332 DirectedPerceptionPTU::min_tilt()
00333 {
00334 return __tilt_lower_limit;
00335 }
00336
00337
00338
00339
00340
00341
00342
00343
00344 void
00345 DirectedPerceptionPTU::get_limits(float &pan_min, float &pan_max,
00346 float &tilt_min, float &tilt_max)
00347 {
00348 pan_min = pan_ticks2rad(__pan_lower_limit);
00349 pan_max = pan_ticks2rad(__tilt_upper_limit);
00350 tilt_min = tilt_ticks2rad(__tilt_lower_limit);
00351 tilt_max = tilt_ticks2rad(__tilt_upper_limit);
00352 }
00353
00354
00355
00356 void
00357 DirectedPerceptionPTU::reset()
00358 {
00359 send(DPPTU_RESET);
00360 }
00361
00362
00363 void
00364 DirectedPerceptionPTU::send(const char *command, int value)
00365 {
00366 snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s%i ", command, value);
00367 write(__obuffer);
00368 if ( ! result_ok() ) {
00369 printf("Writing with value '%s' to PTU failed\n", __obuffer);
00370 }
00371 }
00372
00373
00374 void
00375 DirectedPerceptionPTU::send(const char *command)
00376 {
00377 snprintf(__obuffer, DPPTU_MAX_OBUFFER_SIZE, "%s ", command);
00378 write(__obuffer);
00379 if ( ! result_ok() ) {
00380 printf("Writing '%s' to PTU failed\n", __obuffer);
00381 }
00382 }
00383
00384
00385 void
00386 DirectedPerceptionPTU::write(const char *buffer)
00387 {
00388 printf("Writing '%s'\n", __obuffer);
00389
00390 tcflush( __fd, TCIOFLUSH );
00391 unsigned int buffer_size = strlen(buffer);
00392 int written = ::write(__fd, buffer, buffer_size);
00393 tcdrain(__fd);
00394
00395 if (written < 0) {
00396 printf("Writing '%s' failed: %s\n", buffer, strerror(errno));
00397 } else if ((unsigned int)written != buffer_size) {
00398 printf("Writing '%s' failed, only wrote %i of %u bytes\n", buffer, written, buffer_size);
00399 }
00400 }
00401
00402
00403 bool
00404 DirectedPerceptionPTU::read(char *buffer, unsigned int buffer_size)
00405 {
00406
00407 timeval start, now;
00408 unsigned int diff_msec = 0;
00409 gettimeofday(&start, NULL);
00410
00411 int num_bytes = 0;
00412 ioctl(__fd, FIONREAD, &num_bytes);
00413 while ( ((__timeout_ms == 0) || (diff_msec < __timeout_ms)) && (num_bytes == 0)) {
00414 ioctl(__fd, FIONREAD, &num_bytes);
00415
00416 gettimeofday(&now, NULL);
00417 diff_msec = (now.tv_sec - start.tv_sec) * 1000 + (now.tv_usec - start.tv_usec) / 1000;
00418 usleep(__timeout_ms * 100);
00419 }
00420 if (num_bytes == 0) {
00421 return false;
00422 }
00423 int bytes_read = ::read(__fd, buffer, buffer_size);
00424 if ( bytes_read < 0 ) {
00425 return false;
00426 } else {
00427 if ((unsigned int)bytes_read == buffer_size) {
00428 return true;
00429 } else {
00430 return false;
00431 }
00432 }
00433 }
00434
00435
00436 bool
00437 DirectedPerceptionPTU::result_ok()
00438 {
00439 if ( read(__ibuffer, 1) ) {
00440 if ( __ibuffer[0] == '*' ) {
00441 return true;
00442 }
00443 }
00444
00445 return false;
00446 }
00447
00448
00449 bool
00450 DirectedPerceptionPTU::data_available()
00451 {
00452 int num_bytes = 0;
00453 ioctl(__fd, FIONREAD, &num_bytes);
00454 return (num_bytes > 0);
00455 }
00456
00457
00458 int
00459 DirectedPerceptionPTU::query_int(const char *query_command)
00460 {
00461 send(query_command);
00462 ssize_t read_bytes = read(__ibuffer, DPPTU_MAX_OBUFFER_SIZE);
00463 if ( read_bytes == -1 ) {
00464 throw FileReadException(__device_file, errno, "Querying integer from PTU failed");
00465 } else if (read_bytes == 0) {
00466 return 0;
00467 }
00468 int rv = 0;
00469 sscanf(__ibuffer, "* %i", &rv);
00470 return rv;
00471 }
00472
00473
00474 int
00475 DirectedPerceptionPTU::pan_rad2ticks(float r)
00476 {
00477 if ( __pan_resolution == 0 ) return 0;
00478 return (int)rint(rad2deg(r) * 3600 / __pan_resolution);
00479 }
00480
00481
00482 int
00483 DirectedPerceptionPTU::tilt_rad2ticks(float r)
00484 {
00485 if ( __tilt_resolution == 0 ) return 0;
00486 return (int)rint(rad2deg(r) * 3600 / __tilt_resolution);
00487 }
00488
00489
00490 float
00491 DirectedPerceptionPTU::pan_ticks2rad(int ticks)
00492 {
00493 if ( __pan_resolution == 0 ) return 0;
00494 return deg2rad(ticks * __pan_resolution / 3600);
00495 }
00496
00497
00498 float
00499 DirectedPerceptionPTU::tilt_ticks2rad(int ticks)
00500 {
00501 if ( __tilt_resolution == 0 ) return 0;
00502 return deg2rad(ticks * __tilt_resolution / 3600);
00503 }