26 #include <core/exceptions/system.h> 27 #include <utils/math/angle.h> 29 #include <sys/types.h> 33 #include <sys/ioctl.h> 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);
virtual void set_pan_tilt(int pan, int tilt)
Set pan and tilt in motor ticks.
virtual int get_pan()
Get current pan in motor ticks.
Fawkes library namespace.
virtual void set_pan(int pan)
Set pan in motor ticks.
virtual ~DirectedPerceptionPTU()
Destructor.
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
virtual void set_tilt(int tilt)
Set tilt in motor ticks.
virtual void get_limits(float &pan_min, float &pan_max, float &tilt_min, float &tilt_max)
Get position limits in radians.
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
virtual void stop_motion()
Stop currently running motion.
virtual void reset()
Reset the PTU.
Base class for exceptions in Fawkes.
virtual int max_tilt()
Get maximum tilt in motor ticks.
virtual void get_pan_tilt(int &pan, int &tilt)
Get current position in motor ticks.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
virtual int min_tilt()
Get minimum tilt in motor ticks.
virtual int min_pan()
Get minimum pan in motor ticks.
virtual int max_pan()
Get maximum pan in motor ticks.
DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms=10)
Constructor.
virtual int get_tilt()
Get current tilt in motor ticks.