26 #include <core/exceptions/system.h>
27 #include <sys/ioctl.h>
30 #include <sys/types.h>
31 #include <utils/math/angle.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 device_file_ = strdup(device_file);
101 timeout_ms_ = timeout_ms;
114 DirectedPerceptionPTU::open()
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");
124 struct termios param;
126 if (tcgetattr(fd_, ¶m) != 0) {
128 throw Exception(
"DP PTU: Cannot get parameters");
132 if (cfsetspeed(¶m, B9600) == -1) {
134 throw Exception(
"DP PTU: Cannot set speed");
138 cfsetospeed(¶m, B9600);
139 cfsetispeed(¶m, B9600);
142 param.c_cflag |= (CLOCAL | CREAD);
143 param.c_cflag &= ~CSIZE;
144 param.c_cflag |= CS8;
145 param.c_cflag &= ~PARENB;
146 param.c_cflag &= ~CSTOPB;
149 param.c_iflag &= ~(INPCK | ISTRIP);
150 param.c_iflag &= ~(IXON | IXOFF | IXANY);
152 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
154 param.c_cc[VTIME] = 1;
155 param.c_cc[VMIN] = 0;
157 if (tcsetattr(fd_, TCSANOW, ¶m) != 0) {
159 throw Exception(
"DP PTU: Cannot set parameters");
165 send(DPPTU_ECHO_DISABLE);
166 send(DPPTU_ASCII_TERSE);
170 pan_resolution_ = query_int(DPPTU_PAN_RESOLUTION);
171 tilt_resolution_ = query_int(DPPTU_TILT_RESOLUTION);
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);
182 DirectedPerceptionPTU::close()
194 send(DPPTU_HALT_ALL);
203 send(DPPTU_PAN_ABSPOS, pan);
212 send(DPPTU_TILT_ABSPOS, tilt);
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_;
231 send(DPPTU_PAN_ABSPOS, pan);
232 send(DPPTU_TILT_ABSPOS, tilt);
242 set_pan_tilt(pan_rad2ticks(pan), tilt_rad2ticks(tilt));
252 pan = query_int(DPPTU_PAN_ABSPOS);
253 tilt = query_int(DPPTU_TILT_ABSPOS);
263 int tpan = 0, ttilt = 0;
265 tpan = query_int(DPPTU_PAN_ABSPOS);
266 ttilt = query_int(DPPTU_TILT_ABSPOS);
268 pan = pan_ticks2rad(tpan);
269 tilt = tilt_ticks2rad(ttilt);
278 return query_int(DPPTU_PAN_ABSPOS);
287 return query_int(DPPTU_TILT_ABSPOS);
296 return pan_upper_limit_;
305 return pan_lower_limit_;
314 return tilt_upper_limit_;
323 return tilt_lower_limit_;
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_);
349 DirectedPerceptionPTU::send(
const char *command,
int value)
351 snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE,
"%s%i ", command, value);
354 printf(
"Writing with value '%s' to PTU failed\n", obuffer_);
359 DirectedPerceptionPTU::send(
const char *command)
361 snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE,
"%s ", command);
364 printf(
"Writing '%s' to PTU failed\n", obuffer_);
369 DirectedPerceptionPTU::write(
const char *buffer)
371 printf(
"Writing '%s'\n", obuffer_);
373 tcflush(fd_, TCIOFLUSH);
374 unsigned int buffer_size = strlen(buffer);
375 int written = ::write(fd_, buffer, buffer_size);
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);
386 DirectedPerceptionPTU::read(
char *buffer,
unsigned int buffer_size)
390 unsigned int diff_msec = 0;
391 gettimeofday(&start, NULL);
394 ioctl(fd_, FIONREAD, &num_bytes);
395 while (((timeout_ms_ == 0) || (diff_msec < timeout_ms_)) && (num_bytes == 0)) {
396 ioctl(fd_, FIONREAD, &num_bytes);
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);
402 if (num_bytes == 0) {
405 ssize_t bytes_read = ::read(fd_, buffer, buffer_size);
406 if (bytes_read < 0) {
409 return (bytes_read > 0);
414 DirectedPerceptionPTU::result_ok()
416 if (read(ibuffer_, 1)) {
417 if (ibuffer_[0] ==
'*') {
426 DirectedPerceptionPTU::data_available()
429 ioctl(fd_, FIONREAD, &num_bytes);
430 return (num_bytes > 0);
434 DirectedPerceptionPTU::query_int(
const char *query_command)
437 bool ok = read(ibuffer_, DPPTU_MAX_IBUFFER_SIZE);
439 throw Exception(
"DP PTU: failed to query integer");
442 if (sscanf(ibuffer_,
"* %i", &intrv) <= 0) {
443 throw Exception(errno,
"DP PTU: failed to query int");
449 DirectedPerceptionPTU::pan_rad2ticks(
float r)
451 if (pan_resolution_ == 0)
453 return (
int)rint(
rad2deg(r) * 3600 / pan_resolution_);
457 DirectedPerceptionPTU::tilt_rad2ticks(
float r)
459 if (tilt_resolution_ == 0)
461 return (
int)rint(
rad2deg(r) * 3600 / tilt_resolution_);
465 DirectedPerceptionPTU::pan_ticks2rad(
int ticks)
467 if (pan_resolution_ == 0)
469 return deg2rad(ticks * pan_resolution_ / 3600);
473 DirectedPerceptionPTU::tilt_ticks2rad(
int ticks)
475 if (tilt_resolution_ == 0)
477 return deg2rad(ticks * tilt_resolution_ / 3600);
virtual int get_tilt()
Get current tilt in motor ticks.
DirectedPerceptionPTU(const char *device_file, unsigned int timeout_ms=10)
Constructor.
virtual void get_pan_tilt(int &pan, int &tilt)
Get current position in motor ticks.
virtual int get_pan()
Get current pan in motor ticks.
virtual void reset()
Reset the PTU.
virtual void set_pan(int pan)
Set pan in motor ticks.
virtual void stop_motion()
Stop currently running motion.
virtual int max_pan()
Get maximum pan in motor ticks.
virtual void set_pan_tilt_rad(float pan, float tilt)
Set pan and tilt in radians.
virtual ~DirectedPerceptionPTU()
Destructor.
virtual void get_pan_tilt_rad(float &pan, float &tilt)
Get pan/tilt in radians.
virtual int max_tilt()
Get maximum tilt in motor ticks.
virtual int min_pan()
Get minimum pan in motor ticks.
virtual int min_tilt()
Get minimum 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_tilt(int tilt)
Set tilt in motor ticks.
virtual void set_pan_tilt(int pan, int tilt)
Set pan and tilt in motor ticks.
Base class for exceptions in Fawkes.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float rad2deg(float rad)
Convert an angle given in radians to degrees.