Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * dp_ptu.cpp - Controller for Directed Perception, Inc. Pan-Tilt Unit on B21 00004 * 00005 * Created: Wed Nov 29 23:05:49 2006 00006 * Copyright 2005-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 /** @class DirectedPerceptionPTU "dp_ptu.h" 00045 * DirectedPerception PTU implementation. 00046 * Control object to use the DirectedPerception PTU Pan/Tilt unit mounted 00047 * on carl. 00048 * 00049 * @author Tim Niemueller 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 /** Constructor. 00095 * @param device_file serial device file (e.g. /dev/ttyS0) 00096 * @param timeout_ms timeout for read operations in miliseconds 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 /** Destructor. */ 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 // set serial line options 00144 param.c_cflag |= ( CLOCAL | CREAD ); // set to local and enable the receiver 00145 param.c_cflag &= ~CSIZE; // mask character size bits 00146 param.c_cflag |= CS8; // select 8 data bits 00147 param.c_cflag &= ~PARENB; // no parity 00148 param.c_cflag &= ~CSTOPB; // 1 stop bit 00149 00150 // set input options 00151 param.c_iflag &= ~( INPCK | ISTRIP ); // no input parity checking 00152 param.c_iflag &= ~( IXON | IXOFF | IXANY ); // no software flow control 00153 00154 param.c_lflag &= ~( ICANON | ECHO | ECHOE | ISIG ); 00155 00156 param.c_cc[ VTIME ] = 1; // wait for a tenth of a second for data 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 // get initial values 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 /** Stop currently running motion. */ 00194 void 00195 DirectedPerceptionPTU::stop_motion() 00196 { 00197 send(DPPTU_HALT_ALL); 00198 } 00199 00200 00201 /** Set pan in motor ticks. 00202 * @param pan pan position in ticks 00203 */ 00204 void 00205 DirectedPerceptionPTU::set_pan(int pan) 00206 { 00207 send(DPPTU_PAN_ABSPOS, pan); 00208 } 00209 00210 00211 /** Set tilt in motor ticks. 00212 * @param tilt tilt position in ticks 00213 */ 00214 void 00215 DirectedPerceptionPTU::set_tilt(int tilt) 00216 { 00217 send(DPPTU_TILT_ABSPOS, tilt); 00218 } 00219 00220 00221 /** Set pan and tilt in motor ticks. 00222 * @param pan pan position in ticks 00223 * @param tilt tilt position in ticks 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 /** Set pan and tilt in radians. 00239 * @param pan pan position rad 00240 * @param tilt tilt position rad 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 /** Get current position in motor ticks. 00250 * @param pan upon return contains current pan position in motor ticks 00251 * @param tilt upon return contains current tilt position in motor ticks 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 /** Get pan/tilt in radians. 00262 * @param pan upon return contains current pan position in radians 00263 * @param tilt upon return contains current tilt position in radians 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 /** Get current pan in motor ticks. 00279 * @return current pan in motor ticks 00280 */ 00281 int 00282 DirectedPerceptionPTU::get_pan() 00283 { 00284 return query_int(DPPTU_PAN_ABSPOS); 00285 } 00286 00287 00288 /** Get current tilt in motor ticks. 00289 * @return current tilt in motor ticks 00290 */ 00291 int 00292 DirectedPerceptionPTU::get_tilt() 00293 { 00294 return query_int(DPPTU_TILT_ABSPOS); 00295 } 00296 00297 /** Get maximum pan in motor ticks. 00298 * @return maximum pan in motor ticks 00299 */ 00300 int 00301 DirectedPerceptionPTU::max_pan() 00302 { 00303 return __pan_upper_limit; 00304 } 00305 00306 00307 /** Get minimum pan in motor ticks. 00308 * @return minimum pan in motor ticks 00309 */ 00310 int 00311 DirectedPerceptionPTU::min_pan() 00312 { 00313 return __pan_lower_limit; 00314 00315 } 00316 00317 00318 /** Get maximum tilt in motor ticks. 00319 * @return maximum tilt in motor ticks 00320 */ 00321 int 00322 DirectedPerceptionPTU::max_tilt() 00323 { 00324 return __tilt_upper_limit; 00325 } 00326 00327 00328 /** Get minimum tilt in motor ticks. 00329 * @return minimum tilt in motor ticks 00330 */ 00331 int 00332 DirectedPerceptionPTU::min_tilt() 00333 { 00334 return __tilt_lower_limit; 00335 } 00336 00337 00338 /** Get position limits in radians. 00339 * @param pan_min upon return contains minimum pan in radians 00340 * @param pan_max upon return contains maximum pan in radians 00341 * @param tilt_min upon return contains minimum tilt in radians 00342 * @param tilt_max upon return contains maximum tilt in radians 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 /** Reset the PTU. */ 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 // wait for message 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 }