26#include <core/exceptions/system.h>
31#include <utils/math/angle.h>
52const char *DirectedPerceptionPTU::DPPTU_PAN_ABSPOS =
"PP";
53const char *DirectedPerceptionPTU::DPPTU_TILT_ABSPOS =
"TP";
54const char *DirectedPerceptionPTU::DPPTU_PAN_RELPOS =
"PO";
55const char *DirectedPerceptionPTU::DPPTU_TILT_RELPOS =
"TO";
56const char *DirectedPerceptionPTU::DPPTU_PAN_RESOLUTION =
"PR";
57const char *DirectedPerceptionPTU::DPPTU_TILT_RESOLUTION =
"TR";
58const char *DirectedPerceptionPTU::DPPTU_PAN_MIN =
"PN";
59const char *DirectedPerceptionPTU::DPPTU_PAN_MAX =
"PX";
60const char *DirectedPerceptionPTU::DPPTU_TILT_MIN =
"TN";
61const char *DirectedPerceptionPTU::DPPTU_TILT_MAX =
"TX";
62const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_QUERY =
"L";
63const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_ENABLE =
"LE";
64const char *DirectedPerceptionPTU::DPPTU_LIMITENFORCE_DISABLE =
"LD";
65const char *DirectedPerceptionPTU::DPPTU_IMMEDIATE_EXECUTION =
"I";
66const char *DirectedPerceptionPTU::DPPTU_SLAVED_EXECUTION =
"S";
67const char *DirectedPerceptionPTU::DPPTU_AWAIT_COMPLETION =
"A";
68const char *DirectedPerceptionPTU::DPPTU_HALT_ALL =
"H";
69const char *DirectedPerceptionPTU::DPPTU_HALT_PAN =
"HP";
70const char *DirectedPerceptionPTU::DPPTU_HALT_TILT =
"HT";
71const char *DirectedPerceptionPTU::DPPTU_PAN_SPEED =
"PS";
72const char *DirectedPerceptionPTU::DPPTU_TILT_SPEED =
"TS";
73const char *DirectedPerceptionPTU::DPPTU_PAN_ACCEL =
"PA";
74const char *DirectedPerceptionPTU::DPPTU_TILT_ACCEL =
"TA";
75const char *DirectedPerceptionPTU::DPPTU_PAN_BASESPEED =
"PB";
76const char *DirectedPerceptionPTU::DPPTU_TILT_BASESPEED =
"TB";
77const char *DirectedPerceptionPTU::DPPTU_PAN_UPPER_SPEED_LIMIT =
"PU";
78const char *DirectedPerceptionPTU::DPPTU_PAN_LOWER_SPEED_LIMIT =
"PL";
79const char *DirectedPerceptionPTU::DPPTU_TILT_UPPER_SPEED_LIMIT =
"TU";
80const char *DirectedPerceptionPTU::DPPTU_TILT_LOWER_SPEED_LIMIT =
"TL";
81const char *DirectedPerceptionPTU::DPPTU_RESET =
"R";
82const char *DirectedPerceptionPTU::DPPTU_STORE =
"DS";
83const char *DirectedPerceptionPTU::DPPTU_RESTORE =
"DR";
84const char *DirectedPerceptionPTU::DPPTU_FACTORY_RESET =
"DF";
85const char *DirectedPerceptionPTU::DPPTU_ECHO_QUERY =
"E";
86const char *DirectedPerceptionPTU::DPPTU_ECHO_ENABLE =
"EE";
87const char *DirectedPerceptionPTU::DPPTU_ECHO_DISABLE =
"ED";
88const char *DirectedPerceptionPTU::DPPTU_ASCII_VERBOSE =
"FV";
89const char *DirectedPerceptionPTU::DPPTU_ASCII_TERSE =
"FT";
90const char *DirectedPerceptionPTU::DPPTU_ASCII_QUERY =
"F";
91const char *DirectedPerceptionPTU::DPPTU_VERSION =
"V";
99 device_file_ = strdup(device_file);
101 timeout_ms_ = timeout_ms;
114DirectedPerceptionPTU::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);
182DirectedPerceptionPTU::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);
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_);
349DirectedPerceptionPTU::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_);
359DirectedPerceptionPTU::send(
const char *command)
361 snprintf(obuffer_, DPPTU_MAX_OBUFFER_SIZE,
"%s ", command);
364 printf(
"Writing '%s' to PTU failed\n", obuffer_);
369DirectedPerceptionPTU::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);
386DirectedPerceptionPTU::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);
414DirectedPerceptionPTU::result_ok()
416 if (read(ibuffer_, 1)) {
417 if (ibuffer_[0] ==
'*') {
426DirectedPerceptionPTU::data_available()
429 ioctl(fd_, FIONREAD, &num_bytes);
430 return (num_bytes > 0);
434DirectedPerceptionPTU::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");
449DirectedPerceptionPTU::pan_rad2ticks(
float r)
451 if (pan_resolution_ == 0)
453 return (
int)rint(
rad2deg(r) * 3600 / pan_resolution_);
457DirectedPerceptionPTU::tilt_rad2ticks(
float r)
459 if (tilt_resolution_ == 0)
461 return (
int)rint(
rad2deg(r) * 3600 / tilt_resolution_);
465DirectedPerceptionPTU::pan_ticks2rad(
int ticks)
467 if (pan_resolution_ == 0)
469 return deg2rad(ticks * pan_resolution_ / 3600);
473DirectedPerceptionPTU::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.