PTUDriver.cpp
Go to the documentation of this file.
1 
8 #include "driver/PTUDriver.h"
9 
10 namespace asr_flir_ptu_driver {
11 
14 
15 PTUDriver::PTUDriver(const char* port, int baud, bool speed_control) {
16  ROS_DEBUG("PTUDriverConstructor");
18  distance_factor = 10;
19  this->speed_control = speed_control;
20 
21  #ifndef __PTU_FREE_INCLUDED__
22  set_baud_rate(baud);
23  COMstream = open_host_port((char*)port);
24  if (isConnected()) {
25  char c = reset_ptu();
26  ROS_DEBUG("ptu_status: %d", c);
27  char d = reset_PTU_parser(1000);
28  ROS_DEBUG("reset_PTU_parser: %d", d);
29  } else {
30  ROS_ERROR("PTU is not connected!");
31  exit(1);
32  }
33  #endif // __PTU_FREE_INCLUDED__
34  #ifdef __PTU_FREE_INCLUDED__
35  long return_code = free_ptu.setNewSerialConnection(port, baud);
36  ROS_DEBUG("Free PTU serial connection set with return code: %ld", return_code);
37  #endif // __PTU_FREE_INCLUDED__
38 
39  /*
40  * With the PTU comes a file, test.c, to be found with the purchasable driver version for the flir ptu.
41  * In Line 632 a test function for the resolution is declared.
42  * The value get_current delivers is devided by 3600 there to get min arc
43  * Therefore it needs to be devided by another 60 to recieve degree.
44  *
45  * Be carefull: Calculation shown in the manual is NOT correct
46  * If PTUFree is used the result that is returned via serial port and via PTUFree sticks to the manual,
47  * but gets scaled by 60 in the code of this class to match the format that ptu.h returns
48  */
49  pan_resolution = get_current(PAN, RESOLUTION) / (3600 * 60.0);
50  tilt_resolution = get_current(TILT, RESOLUTION) / (3600 * 60.0);
51 
52  //Important, let it in, do not change (other mode caused problems with our ptu)
54 
55  setSpeedControlMode(speed_control);
56  ROS_DEBUG("port: %s, baud: %d, speed_control: %d", port, baud, speed_control);
58 }
59 
61  ROS_DEBUG("This constructor is only for the mock!");
62 }
63 
65  //empty
66 }
67 
68 
70  #ifndef __PTU_FREE_INCLUDED__
71  return COMstream > 0;
72  #endif
73  #ifdef __PTU_FREE_INCLUDED__
74  return free_ptu.isOpen();
75  #endif
76 }
77 
78 void PTUDriver::setComputationTolerance(double computation_tolerance) {
79  this->double_computation_tolerance = computation_tolerance;
80 }
81 
83  this->distance_factor = distance_factor;
84 }
85 
86 
87 void PTUDriver::setSettings(int pan_base, int tilt_base, int pan_speed, int tilt_speed,
88  int pan_upper, int tilt_upper,int pan_accel, int tilt_accel,
89  int pan_hold, int tilt_hold, int pan_move, int tilt_move) {
91  char return_val;
92  bool no_error_occured = true;
93  if(no_error_occured)
94  {
95  return_val = set_desired(PAN, BASE, (short int *)&pan_base, ABSOLUTE);
96  ROS_DEBUG_STREAM("set_desired(PAN, BASE, " << pan_base << ", ABSOLUTE) returned " << getErrorString(return_val) );
97  if(!checkReturnCode(return_val)) no_error_occured = false;
98  }
99  if(no_error_occured)
100  {
101  return_val = set_desired(PAN, UPPER_SPEED_LIMIT, (short int *)&pan_upper, ABSOLUTE);
102  ROS_DEBUG_STREAM("set_desired(PAN, UPPER_SPEED_LIMIT, " << pan_upper << ", ABSOLUTE) returned " << getErrorString(return_val) );
103  if(!checkReturnCode(return_val)) no_error_occured = false;
104  }
105  if(no_error_occured)
106  {
107  return_val = set_desired(PAN, SPEED, (short int *)&pan_speed, ABSOLUTE);
108  ROS_DEBUG_STREAM("set_desired(PAN, SPEED, " <<pan_speed<< ", ABSOLUTE) returned " << getErrorString(return_val) );
109  if(!checkReturnCode(return_val)) no_error_occured = false;
110  }
111 
112  if(no_error_occured)
113  {
114  return_val = set_desired(PAN, ACCELERATION, (short int *)&pan_accel, ABSOLUTE);
115  ROS_DEBUG_STREAM("set_desired(PAN, ACCELERATION, " << pan_accel << ", ABSOLUTE) returned " << getErrorString(return_val) );
116  if(!checkReturnCode(return_val)) no_error_occured = false;
117  }
118 
119  if(no_error_occured)
120  {
121  return_val = set_desired(PAN, HOLD_POWER_LEVEL, &POW_VAL_HOLD[pan_hold], ABSOLUTE);
122  ROS_DEBUG_STREAM("set_desired(PAN, HOLD_POWER_LEVEL, " << POW_VAL_HOLD[pan_hold] << ", ABSOLUTE) returned " << getErrorString(return_val) );
123  if(!checkReturnCode(return_val)) no_error_occured = false;
124  }
125 
126  if(no_error_occured)
127  {
128  return_val = set_desired(PAN, MOVE_POWER_LEVEL, &POW_VAL_MOVE[pan_move], ABSOLUTE);
129  ROS_DEBUG_STREAM("set_desired(PAN, MOVE_POWER_LEVEL, " << POW_VAL_MOVE[pan_move] <<", ABSOLUTE) returned " << getErrorString(return_val) );
130  if(!checkReturnCode(return_val)) no_error_occured = false;
131  }
132 
133  if(no_error_occured)
134  {
135  return_val = set_desired(TILT, BASE, (short int *)&tilt_base, ABSOLUTE);
136  ROS_DEBUG_STREAM("set_desired(TILT, BASE, " << tilt_base << ", ABSOLUTE) returned " << getErrorString(return_val) );
137  if(!checkReturnCode(return_val)) no_error_occured = false;
138  }
139 
140  if(no_error_occured)
141  {
142  return_val = set_desired(TILT, UPPER_SPEED_LIMIT, (short int *)&tilt_upper, ABSOLUTE);
143  ROS_DEBUG_STREAM("set_desired(TILT, UPPER_SPEED_LIMIT, " << tilt_upper << ", ABSOLUTE) returned " << getErrorString(return_val) );
144  if(!checkReturnCode(return_val)) no_error_occured = false;
145  }
146 
147  if(no_error_occured)
148  {
149  if(no_error_occured) return_val = set_desired(TILT, SPEED, (short int *)&tilt_speed, ABSOLUTE);
150  ROS_DEBUG_STREAM("set_desired(TILT, SPEED, " <<tilt_speed<< ", ABSOLUTE) returned " << getErrorString(return_val) );
151  if(!checkReturnCode(return_val)) no_error_occured = false;
152  }
153 
154 
155  if(no_error_occured)
156  {
157  return_val = set_desired(TILT, ACCELERATION, (short int *)&tilt_accel, ABSOLUTE);
158  ROS_DEBUG_STREAM("set_desired(TILT, ACCELERATION, " << tilt_accel << ", ABSOLUTE) returned " << getErrorString(return_val) );
159  if(!checkReturnCode(return_val)) no_error_occured = false;
160  }
161 
162  if(no_error_occured)
163  {
164  return_val = set_desired(TILT, HOLD_POWER_LEVEL, &POW_VAL_HOLD[tilt_hold], ABSOLUTE);
165  ROS_DEBUG_STREAM("set_desired(TILT, HOLD_POWER_LEVEL, " << POW_VAL_HOLD[tilt_hold] << ", ABSOLUTE) returned " << getErrorString(return_val) );
166  if(!checkReturnCode(return_val)) no_error_occured = false;
167  }
168 
169  if(no_error_occured)
170  {
171  return_val = set_desired(TILT, MOVE_POWER_LEVEL, &POW_VAL_MOVE[tilt_move], ABSOLUTE);
172  ROS_DEBUG_STREAM("set_desired(TILT, MOVE_POWER_LEVEL, " << POW_VAL_MOVE[tilt_move] <<", ABSOLUTE) returned " << getErrorString(return_val) );
173  if(!checkReturnCode(return_val)) no_error_occured = false;
174  }
175 
176 
177  if(!no_error_occured)
178  {
179  ROS_ERROR("Error occured while setting new settings! Old settings will be restored.");
181  setValuesToBackupValues(pan_base, tilt_base, pan_speed, tilt_speed, pan_upper, tilt_upper, pan_accel, tilt_accel, pan_hold, tilt_hold, pan_move, tilt_move);
182  }
183 }
184 
185 
186 
187 void PTUDriver::setValuesToBackupValues(int & pan_base, int & tilt_base, int & pan_speed, int & tilt_speed,
188  int & pan_upper, int & tilt_upper, int & pan_accel, int & tilt_accel,
189  int & pan_hold, int & tilt_hold, int & pan_move, int & tilt_move)
190 {
191  pan_base = (int) backup_pan_base;
192  pan_upper = (int) backup_pan_upper;
193  pan_speed = (int) backup_pan_speed;
194  pan_accel = (int) backup_pan_accel;
195  if(POW_VAL_HOLD[0] == backup_pan_hold) {
196  pan_hold = 0;
197  }
198  else if (POW_VAL_HOLD[1] == backup_pan_hold) {
199  pan_hold = 1;
200  }
201  else {
202  pan_hold = 2;
203  }
204 
205  if(POW_VAL_MOVE[0] == backup_pan_move) {
206  pan_move = 0;
207  }
208  else if (POW_VAL_MOVE[1] == backup_pan_move) {
209  pan_move = 1;
210  }
211  else {
212  pan_move = 2;
213  }
214  tilt_base = (int) backup_tilt_base;
215  tilt_upper = (int) backup_tilt_upper;
216  tilt_speed = (int) backup_tilt_speed;
217  tilt_accel = (int) backup_tilt_accel;
218  if(POW_VAL_HOLD[0] == backup_tilt_hold) {
219  tilt_hold = 0;
220  }
221  else if (POW_VAL_HOLD[1] == backup_tilt_hold) {
222  tilt_hold = 1;
223  }
224  else {
225  tilt_hold = 2;
226  }
227 
228  if(POW_VAL_MOVE[0] == backup_tilt_move) {
229  tilt_move = 0;
230  }
231  else if (POW_VAL_MOVE[1] == backup_tilt_move) {
232  tilt_move = 1;
233  }
234  else {
235  tilt_move = 2;
236  }
237 }
238 
239 
241 {
254 }
255 
256 //Possible extensions for error handling could be done here (e.g. extra info output)
257 bool PTUDriver::checkReturnCode(char return_code) {
258  if(return_code == PTU_OK) return true;
259  return false;
260 }
261 
262 
264  set_desired(PAN, BASE, (short int *)&backup_pan_base, ABSOLUTE);
266  set_desired(PAN, SPEED, (short int *)&backup_pan_speed, ABSOLUTE);
270  set_desired(TILT, BASE, (short int *)&backup_tilt_base, ABSOLUTE);
272  set_desired(TILT, SPEED, (short int *)&backup_tilt_speed, ABSOLUTE);
276 }
277 
278 long PTUDriver::getLimitAngle(char pan_or_tilt, char upper_or_lower) {
279  if(pan_or_tilt == 'p') {
280  if (upper_or_lower == 'l') {
282  }
283  else if (upper_or_lower == 'u') {
285  }
286  else {
287  return std::numeric_limits<double>::max();
288  }
289  }
290  else if (pan_or_tilt == 't') {
291  if (upper_or_lower == 'l') {
293  }
294  else if (upper_or_lower == 'u') {
296  }
297  else {
298  return std::numeric_limits<double>::max();
299  }
300  }
301  else {
302  return std::numeric_limits<double>::max();
303  }
304 }
305 
306 
307 
308 void PTUDriver::setForbiddenAreas(std::vector< std::map< std::string, double> > forbidden_areas) {
309  std::vector< std::map< std::string, double> > legit_forbidden_areas;
310  for (unsigned int i = 0; i < forbidden_areas.size(); i++)
311  {
312  std::map< std::string, double> area = forbidden_areas.at(i);
313  double area_pan_min = area["pan_min"];
314  double area_pan_max = area["pan_max"];
315  double area_tilt_min = area["tilt_min"];
316  double area_tilt_max = area["tilt_max"];
317  if ( area_pan_min <= area_pan_max &&
318  area_tilt_min <= area_tilt_max)
319  {
320  legit_forbidden_areas.push_back(area);
321  }
322  else
323  {
324  ROS_ERROR("Forbidden Area with pan_min %f, pan_max %f, tilt_min %f and tilt_max %f is not valid. One of the max values is larger than the related min value.\n", area_pan_min, area_pan_max, area_tilt_min, area_tilt_max);
325  }
326  }
327  this->forbidden_areas = legit_forbidden_areas;
329 }
330 
331 void PTUDriver::setAbsoluteAngleSpeeds(double pan_speed, double tilt_speed) {
332  ROS_DEBUG("Driver::SetAbsoluteAngleSpeed()");
333  signed short pan_spd = (signed short) convertPanFromAngleToPosition(pan_speed);
334  signed short tilt_spd = (signed short) convertTiltFromAngleToPosition(tilt_speed);
335 
336  if ((getCurrentAngle(PAN) <= pan_min && pan_spd < 0) ||
337  (getCurrentAngle(PAN) >= pan_max && pan_spd > 0)) {
338  pan_spd = 0;
339  ROS_DEBUG("Driver::PanSpeed=0");
340  }
341  if ((getCurrentAngle(TILT) <= tilt_min && tilt_spd < 0) ||
342  (getCurrentAngle(TILT) >= tilt_max && tilt_spd > 0)) {
343  tilt_spd = 0;
344  ROS_DEBUG("Driver::TiltSpeed=0");
345  }
346  setAbsoluteAngleSpeeds(pan_spd, tilt_spd);
347  ROS_DEBUG("Driver::SetAbsoluteAngleSpeed");
348 }
349 
350 void PTUDriver::setAbsoluteAngleSpeeds(signed short pan_speed, signed short tilt_speed) {
351  char return_code;
352  return_code = set_desired(PAN, SPEED, &pan_speed, ABSOLUTE);
353  ROS_DEBUG_STREAM("set_desired(PAN, SPEED, " << pan_speed << ", ABSOLUTE) returned "
354  << getErrorString(return_code) );
355  return_code = set_desired(TILT, SPEED, &tilt_speed, ABSOLUTE);
356  ROS_DEBUG_STREAM("set_desired(TILT, SPEED, " << tilt_speed << ", ABSOLUTE) returned "
357  << getErrorString(return_code) );
358 
359 }
360 
361 //Do not use if you use a point determinded by checkForPossibleKollision or determineLegitEndPoint because it could lie minimal within a forbidden area (unprecise double calculations) or (normally) directly on the border.
362 bool PTUDriver::isInForbiddenArea(double pan_angle, double tilt_angle)
363 {
364  ROS_DEBUG("Driver::isInForbiddenArea()");
365  for (unsigned int i = 0; i < forbidden_areas.size(); i++)
366  {
367  ROS_DEBUG("area %d", i);
368  std::map< std::string, double> area = forbidden_areas.at(i);
369  double area_pan_min = area["pan_min"];
370  double area_pan_max = area["pan_max"];
371  double area_tilt_min = area["tilt_min"];
372  double area_tilt_max = area["tilt_max"];
373  ROS_DEBUG_STREAM("Checking forbidden area " << i << " with pan_min " << area_pan_min << ", pan_max " << area_pan_max << ", tilt_min " << area_tilt_min << ", tilt_max " << area_tilt_max);
374 
375  if (area_pan_min < pan_angle && pan_angle < area_pan_max &&
376  area_tilt_min < tilt_angle && tilt_angle < area_tilt_max)
377  {
378  ROS_ERROR("Value lies in forbidden area: %d", i);
379  return true;
380  }
381  }
382  return false;
383 }
384 
385 bool PTUDriver::isWithinPanTiltLimits(double pan, double tilt) {
388  return true;
389  }
390  else{
391  ROS_DEBUG("TILT %f is not between the lower limit %f and the upper limit %f", tilt, convertTiltFromPositionToAngle(tilt_min), convertTiltFromPositionToAngle(tilt_max));
392  }
393  }
394  else {
395  ROS_DEBUG("PAN %f is not between the lower limit %f and the upper limit %f", pan, convertPanFromPositionToAngle(pan_min), convertPanFromPositionToAngle(pan_max));
396  }
397  return false;
398 }
399 
400 bool PTUDriver::setValuesOutOfLimitsButWithinMarginToLimit(double * pan, double * tilt, double margin) {
401  ROS_DEBUG("Before margin check: PAN: %f, TILT: %f\n", *pan, *tilt);
402  long margin_as_position = convertPanFromAngleToPosition(margin);
403  if(((pan_max - pan_min) <= margin_as_position) || ((tilt_max - tilt_min) <= margin_as_position)) {
404 
405  ROS_ERROR("Margin in Positions (%ld) (Degree: (%f)) is too big. Bigger than distance between TILT max and TILT min (%ld Positions) or PAN max and PAN min (%ld Positions)",margin_as_position, margin, (tilt_max - tilt_min), (pan_max - pan_min));
406  return false;
407  }
408  double pan_initial = *pan;
409  double tilt_initial = *tilt;
410  short int pan_as_position = convertPanFromAngleToPosition(*pan);
411  short int tilt_as_position = convertTiltFromAngleToPosition(*tilt);
412  if((pan_as_position < pan_min) && (pan_as_position >= (pan_min - margin_as_position))) {
413  pan_as_position = pan_min;
415  ROS_WARN("PAN was out of limits, but within margin, so instead of the initial value %f the value %f is used", pan_initial, *pan);
416  }
417  else if ((pan_as_position > pan_max) && (pan_as_position <= (pan_max + margin_as_position))) {
418  pan_as_position = pan_max;
420  ROS_WARN("PAN was out of limits, but within margin, so instead of the initial value %f the value %f is used", pan_initial, *pan);
421  }
422  if((tilt_as_position < tilt_min) && (tilt_as_position >= (tilt_min - margin_as_position))) {
423  tilt_as_position = tilt_min;
425  ROS_WARN("TILT was out of limits, but within margin, so instead of the initial value %f the value %f is used", tilt_initial, *tilt);
426  }
427  else if((tilt_as_position > tilt_max) && (tilt_as_position <= (tilt_max + margin_as_position))) {
428  tilt_as_position = tilt_max;
430  ROS_WARN("TILT was out of limits, but within margin, so instead of the initial value %f the value %f is used", tilt_initial, *tilt);
431  }
432 
433  if(isWithinPanTiltLimits(*pan, *tilt)) {
434  ROS_DEBUG("After margin check: PAN: %f, TILT: %f\n", *pan, *tilt);
435  return true;
436  }
437  else {
438  ROS_ERROR("PAN/TILT out of bounds (PAN: %f, TILT: %f)", *pan, *tilt);
439  return false;
440  }
441 }
442 
444  this->pan_min = (double) get_current(PAN, MINIMUM_POSITION);
445  this->pan_max = (double) get_current(PAN, MAXIMUM_POSITION);
446  this->tilt_min = (double) get_current(TILT, MINIMUM_POSITION);
447  this->tilt_max = (double) get_current(TILT, MAXIMUM_POSITION);
448  ROS_DEBUG("Limits set to Hardware Limits: PAN_MINIMUM: %f, PAN_MAXIMUM: %f, TILT_MINIMUM: %f, TILT_MAXIMUM: %f", convertPanFromPositionToAngle(pan_min), convertPanFromPositionToAngle(pan_max), convertTiltFromPositionToAngle(tilt_min), convertTiltFromPositionToAngle(tilt_max));
449 }
450 
451 //No Check should only be used if you use a point that can be minimal within a forbidden area and when you know that getting in touch with the border of the forbidden area is no problem
452 //A typical case for that problem is a point derived from 'checkForPossibleKollision' or 'determineLegitEndPoint' - these can lay onto the border of a forbidden area or minimal within
453 bool PTUDriver::setAbsoluteAngles(double pan_angle, double tilt_angle, bool no_forbidden_area_check) {
454 
455  // Maybe for later: use with path prediction, the problem that the new position must be returned to the layers above is currently only solved for the GUI, not for the PTU Controller
456  //PTU Controller must be modified to support new end points to do so
457 
458  ROS_DEBUG("Driver::SetAbsoluteAngles()");
459  if (isInForbiddenArea(pan_angle, tilt_angle) && !no_forbidden_area_check)
460  {
461  return false;
462  }
463  short int pan_short_angle = convertPanFromAngleToPosition(pan_angle);
464  short int tilt_short_angle = convertTiltFromAngleToPosition(tilt_angle);
465 
466  if ((pan_short_angle < pan_min)
467  || (pan_short_angle > pan_max)
468  || (tilt_short_angle < tilt_min)
469  || (tilt_short_angle > tilt_max))
470  {
471  ROS_ERROR("PAN/TILT angle not within pan/tilt bounds");
472  return false;
473  }
474  char return_code;
475  ROS_DEBUG_STREAM("panShortAngle = " << pan_short_angle);
476  return_code = set_desired(PAN, POSITION, &pan_short_angle, ABSOLUTE);
477  ROS_DEBUG_STREAM("set_desired(PAN, POSITION, " << pan_short_angle << ", ABSOLUTE) returned "
478  << getErrorString(return_code) );
479  ROS_DEBUG_STREAM("tiltShortAngle = " << tilt_short_angle);
480  return_code = set_desired(TILT, POSITION, &tilt_short_angle, ABSOLUTE);
481  ROS_DEBUG_STREAM("set_desired(TILT, POSITION, " << tilt_short_angle << ", ABSOLUTE) returned "
482  << getErrorString(return_code) );
483  ROS_INFO("Successfully set Pan and Tilt. PAN: %f, TILT: %f\n", pan_angle, tilt_angle);
484  return true;
485 }
486 
487 void PTUDriver::setLimitAngles(double pan_min, double pan_max, double tilt_min, double tilt_max) {
488  long pan_min_candidate = convertPanFromAngleToPosition(pan_min);
489  if(pan_min_candidate >= get_current(PAN, MINIMUM_POSITION)) {
490  this->pan_min = pan_min_candidate;
491  }
492  else {
493  this->pan_min = get_current(PAN, MINIMUM_POSITION);
494  ROS_ERROR("Desired PAN_MIN Position was smaller than the hardware limit -> PAN_MIN set to hardware limit");
495  }
496  double pan_max_candidate = convertPanFromAngleToPosition(pan_max);
497  if(pan_max_candidate <= get_current(PAN, MAXIMUM_POSITION)) {
498  this->pan_max = pan_max_candidate;
499  }
500  else {
501  this->pan_max = get_current(PAN, MAXIMUM_POSITION);
502  ROS_ERROR("Desired PAN_MAX Position was greater than the hardware limit -> PAN_MAX set to hardware limit");
503  }
504  double tilt_min_candidate = convertTiltFromAngleToPosition(tilt_min);
505  if(tilt_min_candidate >= get_current(TILT, MINIMUM_POSITION)) {
506  this->tilt_min = tilt_min_candidate;
507  }
508  else {
509  this->tilt_min = get_current(TILT, MINIMUM_POSITION);
510  ROS_ERROR("Desired TILT_MIN Position was smaller than the hardware limit -> TILT_MIN set to hardware limit");
511  }
512  double tilt_max_candidate = convertTiltFromAngleToPosition(tilt_max);
513  if(tilt_max_candidate <= get_current(TILT, MAXIMUM_POSITION)) {
514  this->tilt_max = tilt_max_candidate;
515  }
516  else {
517  this->tilt_max = get_current(TILT, MAXIMUM_POSITION);
518  ROS_ERROR("Desired TILT_MAX Position was greater than the hardware limit -> TILT_MAX set to hardware limit");
519  }
520  ROS_DEBUG("Limits set to Virtual Limits: PAN_MINIMUM: %f, PAN_MAXIMUM: %f, TILT_MINIMUM: %f, TILT_MAXIMUM: %f", pan_min, pan_max, tilt_min, tilt_max);
521 }
522 
524  this->speed_control = speed_control;
525  if (speed_control) {
527  } else {
529  }
530 }
531 
533  return speed_control;
534 }
535 
536 double PTUDriver::getCurrentAngle(char type) {
537  if (type == PAN) {
539  }
540  else if(type == TILT){
542  }
543  else {
544  throw std::exception();
545  }
546 
547 }
548 
549 double PTUDriver::getDesiredAngle(char type) {
550  if (type == PAN) {
552  }
553  else if (type == TILT) {
555  }
556  else {
557  throw std::exception();
558  }
559 }
560 
561 double PTUDriver::getAngleSpeed(char type) {
562  if (type == PAN) {
564  }
565  else if (type == TILT) {
567  }
568  else {
569  throw std::exception();
570  }
571 }
572 
574  return ((long) round(angle / pan_resolution));
575 }
576 
578  return ((double)position) * pan_resolution;
579 }
580 
582  return ((long) round(angle / tilt_resolution));
583 }
584 
586  return ((double)position) * tilt_resolution;
587 }
588 
589 std::string PTUDriver::getErrorString(char status_code)
590 {
591  #ifndef __PTU_FREE_INCLUDED__
592  switch (status_code)
593  {
594  case PTU_OK:
595  return "PTU_OK"; break;
596  case PTU_ILLEGAL_COMMAND:
597  return "PTU_ILLEGAL_COMMAND"; break;
598  case PTU_ILLEGAL_POSITION_ARGUMENT:
599  return "PTU_ILLEGAL_POSITION_ARGUMENT"; break;
600  case PTU_ILLEGAL_SPEED_ARGUMENT:
601  return "PTU_ILLEGAL_SPEED_ARGUMENT"; break;
602  case PTU_ACCEL_TABLE_EXCEEDED:
603  return "PTU_ACCEL_TABLE_EXCEEDED"; break;
604  case PTU_DEFAULTS_EEPROM_FAULT:
605  return "PTU_DEFAULTS_EEPROM_FAULT"; break;
606  case PTU_SAVED_DEFAULTS_CORRUPTED:
607  return "PTU_SAVED_DEFAULTS_CORRUPTED"; break;
608  case PTU_LIMIT_HIT:
609  return "PTU_LIMIT_HIT"; break;
610  case PTU_CABLE_DISCONNECTED:
611  return "PTU_CABLE_DISCONNECTED"; break;
612  case PTU_ILLEGAL_UNIT_ID:
613  return"PTU_ILLEGAL_UNIT_ID"; break;
614  case PTU_ILLEGAL_POWER_MODE:
615  return "PTU_ILLEGAL_POWER_MODE"; break;
616  case PTU_RESET_FAILED:
617  return "PTU_RESET_FAILED"; break;
618  case PTU_ILLEGAL_PARAMETERS:
619  return "PTU_ILLEGAL_PARAMETERS"; break;
620  case PTU_DUART_ERROR:
621  return "PTU_DUART_ERROR"; break;
622  case PTU_ERROR:
623  return "PTU_ERROR"; break;
624  case NOT_SUPPORTED_BY_THIS_FIRMWARE_VERSION:
625  return "NOT_SUPPORTED_BY_THIS_FIRMWARE_VERSION"; break;
626  case PTU_TILT_VANE_OUT_OF_RANGE_ERROR:
627  return "PTU_TILT_VANE_OUT_OF_RANGE_ERROR"; break;
628  default:
629  return "UNDEFINED_RETURN_STATUS_ERROR"; break;
630  }
631  #endif
632  #ifdef __PTU_FREE_INCLUDED__
633  if(status_code == PTU_OK) {
634  return "PTU_OK";
635  }
636  else {
637  return "PTU_ERROR";
638  }
639  #endif
640 }
641 
643  return hasHalted() && reachedGoal();
644 }
645 
647  long current_tilt_speed = get_current(TILT, SPEED);
648  long current_pan_speed = get_current(PAN, SPEED);
649  return (((current_tilt_speed == 0) && (current_pan_speed == 0)));
650 }
651 
652 //TODO: why is computation tolerance not used here? necessary?
654  long current_tilt = get_current(TILT, POSITION);
655  long desired_tilt = get_desired(TILT, POSITION);
656  long current_pan = get_current(PAN, POSITION);
657  long desired_pan = get_desired(PAN, POSITION);
658  return ((current_tilt == desired_tilt) && (current_pan == desired_pan));
659 }
660 
661 
662 /*
663 * Needs to have the form a * x^2 + b * x + c = 0, where x is the variable who needs to be solved and a, b and c are constants
664 *
665 */
666 std::vector<double> PTUDriver::solveSecondDegreePolynomial(double a, double b, double c) {
667  std::vector<double> solutions;
668  //midnight formula used (b +- sqrt(b^2 - 4 * a * c))/2a
669  double second_part_before_root = pow(b, 2) - 4 * a * c;
670  if(second_part_before_root < 0){
671  return solutions;
672  }
673  if(second_part_before_root == 0) {
674  solutions.push_back(-b / (2 * a));
675  return solutions;
676  }
677  double root_part = sqrt(second_part_before_root);
678  solutions.push_back((-b + root_part) / (2 * a));
679  solutions.push_back((-b - root_part) / (2 * a));
680  return solutions;
681 }
682 
683 //Returns a vector consisting of acceleration time (at position 0) and slew speed time (position 1). Do not forget that acceleration time is ONLY for acceleration and not for acceleration and deceleration.
684 //Though, acceleration time and deceleration time are the same because ramp profile is used.
685 //Returns empty vector on error.
686 //All needs to be entered in steps!!!
687 std::vector<double> PTUDriver::getAccelerationTimeAndSlewSpeedTime(double distance_in_steps, double base_speed, double acceleration, double slew_speed) {
688  std::vector<double> solution;
689  if(distance_in_steps == 0.0) {
690  solution.push_back(0.0);
691  solution.push_back(0.0);
692  return solution;
693  }
694  //Because acceleration to BASE SPEED happens instantly according to the manual it needs to be subtracted from the desired speed (slew speed)
695  double speed_to_accelerate = slew_speed - base_speed;
696  double acceleration_time_to_desired_speed = speed_to_accelerate / acceleration;
697  double distance_for_full_acceleration_and_deceleration = 2.0 * (acceleration_time_to_desired_speed * base_speed
698  + 1.0/2.0 * acceleration * pow(acceleration_time_to_desired_speed, 2));
699  //In that case the PTU can not accelerate to full desired speed
700  if(distance_for_full_acceleration_and_deceleration >= distance_in_steps) {
701  std::vector<double> speed_candidates = solveSecondDegreePolynomial(1.0 / acceleration, 2.0 * base_speed / acceleration, -distance_in_steps);
702  if(speed_candidates.size() == 0) {
703  ROS_DEBUG("PAN ACCELERATION TIME CALCULATION FAILED (getAccelerationTime-Method)");
704  return solution;
705  }
706  else if(speed_candidates.size() == 1) {
707  solution.push_back(speed_candidates[0]);
708  return solution;
709  }
710  else {
711  double new_acceleration_time_to_desired_speed_candidate_one = speed_candidates[0] / acceleration;
712  double new_acceleration_time_to_desired_speed_candidate_two = speed_candidates[1] / acceleration;
713  if(speed_candidates[0] > 0.0) {
714  solution.push_back(new_acceleration_time_to_desired_speed_candidate_one);
715  solution.push_back(0.0);
716  return solution;
717  }
718  else {
719  solution.push_back(new_acceleration_time_to_desired_speed_candidate_two);
720  solution.push_back(0.0);
721  return solution;
722  }
723  }
724  }
725  else {
726  solution.push_back(acceleration_time_to_desired_speed);
727  double remaining_distance_at_slew_speed = distance_in_steps - distance_for_full_acceleration_and_deceleration;
728  double time_at_slew_speed = remaining_distance_at_slew_speed / slew_speed;
729  solution.push_back(time_at_slew_speed);
730  return solution;
731  }
732 }
733 
734 
735 //Pan at position 0, tilt at position 1, input in degree, output in degree!
736 std::vector<double> PTUDriver::predictPositionInTime(std::vector<double> start_point, std::vector<double> end_point, double point_in_time) {
737  double pan_distance = end_point[0] - start_point[0];
738  double tilt_distance = end_point[1] - start_point[1];
739  double pan_distance_in_steps = convertPanFromAngleToPosition(pan_distance);
740  double tilt_distance_in_steps = convertTiltFromAngleToPosition(tilt_distance);
741 
744 
745  double pan_complete_time = 2.0 * pan_time[0] + pan_time[1];
746  double tilt_complete_time = 2.0 * tilt_time[0] + tilt_time[1];
747  double max_time = std::max(pan_complete_time, tilt_complete_time);
748  if(max_time <= point_in_time) {
749  return end_point;
750  }
751  else {
752  std::vector<double> pan_tilt_distance;
753  if(pan_time[0] >= point_in_time) {
754  double acceleration_time = point_in_time;
755  double slew_speed_time = 0.0;
756  double decceleration_time = 0.0;
757  if(pan_distance >= 0) {
758  pan_tilt_distance.push_back(convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
759  }
760  else {
761  pan_tilt_distance.push_back(-1.0 * convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
762  }
763  }
764  else if((pan_time[0] + pan_time[1]) >= point_in_time) {
765  double acceleration_time = pan_time[0];
766  double slew_speed_time = point_in_time - pan_time[0];
767  double decceleration_time = 0.0;
768  if(pan_distance >= 0) {
769  pan_tilt_distance.push_back(convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
770  }
771  else {
772  pan_tilt_distance.push_back(-1.0 * convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
773  }
774  }
775  //neccessary because before only max of pan and tilt complete time was checked
776  else if((2.0 * pan_time[0] + pan_time[1]) >= point_in_time){
777  double acceleration_time = pan_time[0];
778  double slew_speed_time = pan_time[1];
779  double decceleration_time = point_in_time - pan_time[0] - pan_time[1];
780  if(pan_distance >= 0) {
781  pan_tilt_distance.push_back(convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
782  }
783  else {
784  pan_tilt_distance.push_back(-1.0 * convertPanFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,true)));
785  }
786  }
787  else {
788  pan_tilt_distance.push_back(end_point[0] - start_point[0]);
789  }
790 
791 
792  if(tilt_time[0] >= point_in_time) {
793  double acceleration_time = point_in_time;
794  double slew_speed_time = 0.0;
795  double decceleration_time = 0.0;
796  if(tilt_distance >= 0) {
797  pan_tilt_distance.push_back(convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
798  }
799  else {
800  pan_tilt_distance.push_back(-1.0 * convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
801  }
802  }
803  else if((tilt_time[0] + tilt_time[1]) >= point_in_time) {
804  double acceleration_time = tilt_time[0];
805  double slew_speed_time = point_in_time - tilt_time[0];
806  double decceleration_time = 0.0;
807  if(tilt_distance >= 0) {
808  pan_tilt_distance.push_back(convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
809  }
810  else {
811  pan_tilt_distance.push_back(-1.0 * convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
812  }
813  }
814  //neccessary because before only max of pan and tilt complete time was checked
815  else if(((2.0 * tilt_time[0] + tilt_time[1]) >= point_in_time)){
816  double acceleration_time = tilt_time[0];
817  double slew_speed_time = tilt_time[1];
818  double decceleration_time = point_in_time - tilt_time[0] - tilt_time[1];
819  if(tilt_distance >= 0) {
820  pan_tilt_distance.push_back(convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
821  }
822  else {
823  pan_tilt_distance.push_back(-1.0 * convertTiltFromPositionToAngle(calculateCoveredDistance(acceleration_time,slew_speed_time,decceleration_time,false)));
824  }
825  }
826  else {
827  pan_tilt_distance.push_back(end_point[1] - start_point[1]);
828  }
829 
830  std::vector<double> new_position;
831  new_position.push_back(start_point[0] + pan_tilt_distance[0]);
832  new_position.push_back(start_point[1] + pan_tilt_distance[1]);
833 
834 
835  return new_position;
836  }
837 }
838 
839 //It is assumed that deccelleration_time is maximum as large as acceleration_time
840 double PTUDriver::calculateCoveredDistance(double acceleration_time, double slew_speed_time, double decceleration_time, bool is_pan) {
841  //char type;
842  long prefetched_base;
843  long prefetched_accel;
844  if(is_pan) {
845  prefetched_base = prefetched_pan_current_base;
846  prefetched_accel = prefetched_pan_desired_acceleration;
847  }
848  else {
849  prefetched_base = prefetched_tilt_current_base;
850  prefetched_accel = prefetched_tilt_desired_acceleration;
851  }
852  double covered_distance = 0.0;
853  double covered_distance_acceleration = prefetched_base * acceleration_time + 1.0/2.0 * prefetched_accel * pow(acceleration_time, 2);
854  covered_distance += covered_distance_acceleration;
855  if(slew_speed_time != 0.0) {
856  double covered_distance_slew_speed = (prefetched_base + acceleration_time * prefetched_accel) * slew_speed_time;
857  covered_distance += covered_distance_slew_speed;
858  }
859  if(decceleration_time != 0) {
860  double covered_distance_decceleration = covered_distance_acceleration - 1.0/2.0 * pow((acceleration_time - decceleration_time),2) * prefetched_accel - (acceleration_time - decceleration_time) * prefetched_base;
861  covered_distance += covered_distance_decceleration;
862  }
863  return covered_distance;
864 }
865 
866 
867 
868 
869 
870 
871 
872 
873 
874 //If non empty: First Value is Pan of intersection Point, second point is tilt of intersection point
875 std::vector<double> PTUDriver::calculatePointOfIntersectionWithForbiddenAreas(std::vector<double> start_point, std::vector<double> end_point) {
876 
877  double distance_from_start = std::numeric_limits<double>::max();
878  std::vector<double> intersection_point;
879  std::vector<double> route_coordiante_form = calculateCoordinateForm(start_point, end_point);
880 
881  for (unsigned int i = 0; i < forbidden_areas.size(); i++) {
882 
883  std::vector<double> first_intersection_point = calculateIntersectionPoint(route_coordiante_form, forbidden_area_first_line_coordinate_forms[i]);
884  std::vector<double> second_intersection_point = calculateIntersectionPoint(route_coordiante_form, forbidden_area_second_line_coordinate_forms[i]);
885  std::vector<double> third_intersection_point = calculateIntersectionPoint(route_coordiante_form, forbidden_area_third_line_coordinate_forms[i]);
886  std::vector<double> fourth_intersection_point = calculateIntersectionPoint(route_coordiante_form, forbidden_area_fourth_line_coordinate_forms[i]);
887  double new_distance_from_start;
888  if((first_intersection_point.size() != 0)
889  && isOnLineSegmentBetweenTwoPoints(start_point, end_point, route_coordiante_form, first_intersection_point, double_computation_tolerance)
891  new_distance_from_start = getVectorLength(start_point, first_intersection_point);
892  if(new_distance_from_start < distance_from_start) {
893  distance_from_start = new_distance_from_start;
894  intersection_point = first_intersection_point;
895  }
896  }
897  if((second_intersection_point.size() != 0)
898  && isOnLineSegmentBetweenTwoPoints(start_point, end_point, route_coordiante_form, second_intersection_point, double_computation_tolerance)
900  new_distance_from_start = getVectorLength(start_point, second_intersection_point);
901  if(new_distance_from_start < distance_from_start) {
902  distance_from_start = new_distance_from_start;
903  intersection_point = second_intersection_point;
904  }
905  }
906  if((third_intersection_point.size() != 0)
907  && isOnLineSegmentBetweenTwoPoints(start_point, end_point, route_coordiante_form, third_intersection_point, double_computation_tolerance)
909  new_distance_from_start = getVectorLength(start_point, third_intersection_point);
910  if(new_distance_from_start < distance_from_start) {
911  distance_from_start = new_distance_from_start;
912  intersection_point = third_intersection_point;
913  }
914  }
915  if((fourth_intersection_point.size() != 0)
916  && isOnLineSegmentBetweenTwoPoints(start_point, end_point, route_coordiante_form, fourth_intersection_point, double_computation_tolerance)
918  new_distance_from_start = getVectorLength(start_point, fourth_intersection_point);
919  if(new_distance_from_start < distance_from_start) {
920  distance_from_start = new_distance_from_start;
921  intersection_point = fourth_intersection_point;
922  }
923  }
924  }
925  if(intersection_point.size() != 0) {
926  printf("Start Point: (%f,%f)\n", start_point[0], start_point[1]);
927  printf("End Point: (%f,%f)\n", end_point[0], end_point[1]);
928  printf("Intersection Point: (%f,%f)\n", intersection_point[0], intersection_point[1]);
929  }
930  return intersection_point;
931 }
932 
933 //must have form a pan + b tilt = c a at 0, b at 1, c at 2; returns empty vector if parallel (-> no intersection point or they are identical)
934 std::vector<double> PTUDriver::calculateIntersectionPoint(std::vector<double> first_line_coordiante_form, std::vector<double> second_line_coordiante_form) {
935  //Calculated with Cramer's rule
936  std::vector<double> intersection_point;
937  double denominator = first_line_coordiante_form[0] * second_line_coordiante_form[1] - first_line_coordiante_form[1] * second_line_coordiante_form[0];
938  if((denominator >= -0.00001 && denominator <= 0.00001)) {
939  return intersection_point;
940  }
941  double pan_intersection_point = first_line_coordiante_form[2] * second_line_coordiante_form[1] - first_line_coordiante_form[1] * second_line_coordiante_form[2];
942  double tilt_intersection_point = first_line_coordiante_form[0] * second_line_coordiante_form[2] - first_line_coordiante_form[2] * second_line_coordiante_form[0];
943  pan_intersection_point /= denominator;
944  tilt_intersection_point /= denominator;
945  intersection_point.push_back(pan_intersection_point);
946  intersection_point.push_back(tilt_intersection_point);
947  return intersection_point;
948 }
949 
950 bool PTUDriver::isOnLineSegmentBetweenTwoPoints(std::vector<double> start_point, std::vector<double> end_point, std::vector<double> line_coordinate_form, std::vector<double> point_to_check, double tolerance) {
951  double distance_from_start_point = sqrt(pow((point_to_check[0] - start_point[0]), 2) + pow((point_to_check[1] - start_point[1]), 2));
952  double distance_from_end_point = sqrt(pow((point_to_check[0] - end_point[0]), 2) + pow((point_to_check[1] - end_point[1]), 2));
953  if((distance_from_start_point <= tolerance) || (distance_from_end_point <= tolerance)) {
954  return true;
955  }
956 
957  //needed to calculate distance with Hesse normal form
958  double normal_vector_length = sqrt(pow(line_coordinate_form[0], 2) + pow(line_coordinate_form[1], 2));
959 
960  //Calculating distance with Hesse normal form
961  double distance_from_line = (1/normal_vector_length) * (point_to_check[0] * line_coordinate_form[0] + point_to_check[1] * line_coordinate_form[1] - line_coordinate_form[2]);
962 
963  if(distance_from_line > tolerance) {
964  return false;
965  }
966 
968  //Check if problem still exists, comment above seems old
969 
970 
971  //We have already determinded that the point is less than tolerance from the line that passes through start_point and end_point.
972  //To determine that the found point lies between start and endpoint we check if it inside the rectange induced by start and endpoint (+ tolerances)
973  //If it is inside the rectangle it is also on the line between both points, because that it is "on" the line was already checked before and all in the rectangle is between the both points
974  double max_pan = std::max(start_point[0], end_point[0]);
975  double min_pan = std::min(start_point[0], end_point[0]);
976  double max_tilt = std::max(start_point[1], end_point[1]);
977  double min_tilt = std::min(start_point[1], end_point[1]);
978  max_pan += tolerance;
979  min_pan -= tolerance;
980  max_tilt += tolerance;
981  min_tilt -= tolerance;
982 
983  if((point_to_check[0] > max_pan) || (point_to_check[0] < min_pan) || (point_to_check[1] > max_tilt) || (point_to_check[1] < min_tilt)) {
984  return false;
985  }
986  else {
987  return true;
988  }
989 }
990 
991 //coordinate form a pan + b tilt = c
992 std::vector<double> PTUDriver::calculateCoordinateForm(std::vector<double> start_point, std::vector<double> end_point) {
993  std::vector<double> coordinate_form;
994  if((start_point[0] == end_point[0]) && (start_point[1] == end_point[1])) {
995  return coordinate_form;
996  }
997  double a = start_point[1] - end_point[1];
998  double b = end_point[0] - start_point[0];
999  double c = end_point[0] * start_point[1] - start_point[0] * end_point[1];
1000  coordinate_form.push_back(a);
1001  coordinate_form.push_back(b);
1002  coordinate_form.push_back(c);
1003  return coordinate_form;
1004 }
1005 
1006 
1007 double PTUDriver::getVectorLength(std::vector<double> input_vector) {
1008  double length = 0.0;
1009  for(unsigned int i = 0; i < input_vector.size(); i++) {
1010  length += pow(input_vector[i], 2);
1011  }
1012  return sqrt(length);
1013 }
1014 
1015 double PTUDriver::getVectorLength(std::vector<double> start_point, std::vector<double> end_point) {
1016  if (start_point.size() != end_point.size()) {
1017  return -1.0;
1018  }
1019  std::vector<double> length_vector(start_point.size());
1020  for (unsigned int i = 0; i < start_point.size(); i++) {
1021  length_vector[i] = end_point[i] - start_point[i];
1022  }
1023  return getVectorLength(length_vector);
1024 }
1025 
1026 std::vector<double> PTUDriver::determineLegitEndPoint(double end_point_pan_candidate, double end_point_tilt_candidate) {
1027  prefetchValues();
1028  std::vector<double> possible_end_point = checkForPossibleKollision(end_point_pan_candidate, end_point_tilt_candidate);
1029  if(possible_end_point.size() == 0) {
1030  std::vector<double> end_point;
1031  end_point.push_back(end_point_pan_candidate);
1032  end_point.push_back(end_point_tilt_candidate);
1033  return end_point;
1034  }
1035  //The path that is taken changes because acceleration time and slew speed time changes - therefore, another collision that did not occure previously can now occure.
1036  while(true) {
1037  std::vector<double> new_possible_end_point = checkForPossibleKollision(possible_end_point[0], possible_end_point[1]);
1038  if(new_possible_end_point.size() == 0) {
1039  return possible_end_point;
1040  }
1041  //In this case, possible_end_point and new_possible_end_point are litterally the same because they only differe because of unprecise double value (from the calculations).
1042  else if (getVectorLength(possible_end_point, new_possible_end_point) < double_computation_tolerance) {
1043  //At this point any of the 2 points could be returned because they only differe less than 'double_computation_tolerance' and therefore are literally the same
1044  return new_possible_end_point;
1045  }
1046  else {
1047  possible_end_point = new_possible_end_point;
1048  }
1049  }
1050 }
1051 
1052 //Returns empty vector if no collision is found, collsision point with forbidden area otherwise
1053 std::vector<double> PTUDriver::checkForPossibleKollision(double new_pan_angle, double new_tilt_angle) {
1054  std::vector<double> start_point;
1056  start_point.push_back(start_pan_value);
1058  start_point.push_back(start_tilt_value);
1059 
1060  std::vector<double> end_point;
1061  end_point.push_back(new_pan_angle);
1062  end_point.push_back(new_tilt_angle);
1063 
1064 
1065  double max_base_speed = std::max(prefetched_pan_current_base, prefetched_tilt_current_base);
1066 
1067  //This is the maximum time needed to switch a pan and a tilt position while the ptu is moving
1068  double time_per_position = 1.0 / max_base_speed;
1069 
1070 
1071 
1072  double time_between_samples = time_per_position * distance_factor;
1073 
1074  double counter = 1.0;
1075  std::vector<double> last_point = start_point;
1076  std::vector<double> intersection_point;
1077  std::vector<double> new_point;
1078  while(true){
1079  new_point = predictPositionInTime(start_point, end_point, (counter * time_between_samples));
1080  if((new_point[0] == last_point[0]) && (new_point[1] == last_point[1])) {
1081  return intersection_point;
1082  }
1083  intersection_point = calculatePointOfIntersectionWithForbiddenAreas(last_point, new_point);
1084  if(intersection_point.size() != 0) {
1085  return intersection_point;
1086  }
1087  else {
1088  last_point = new_point;
1089  counter += 1.0;
1090  }
1091  }
1092 }
1093 
1094 
1096  for (unsigned int i = 0; i < forbidden_areas.size(); i++) {
1097  std::vector<double> max_pan_max_tilt_point;
1098  max_pan_max_tilt_point.push_back(forbidden_areas[i]["pan_max"]);
1099  max_pan_max_tilt_point.push_back(forbidden_areas[i]["tilt_max"]);
1100  max_pan_max_tilt_points.push_back(max_pan_max_tilt_point);
1101  std::vector<double> max_pan_min_tilt_point;
1102  max_pan_min_tilt_point.push_back(forbidden_areas[i]["pan_max"]);
1103  max_pan_min_tilt_point.push_back(forbidden_areas[i]["tilt_min"]);
1104  max_pan_min_tilt_points.push_back(max_pan_min_tilt_point);
1105  std::vector<double> min_pan_max_tilt_point;
1106  min_pan_max_tilt_point.push_back(forbidden_areas[i]["pan_min"]);
1107  min_pan_max_tilt_point.push_back(forbidden_areas[i]["tilt_max"]);
1108  min_pan_max_tilt_points.push_back(min_pan_max_tilt_point);
1109  std::vector<double> min_pan_min_tilt_point;
1110  min_pan_min_tilt_point.push_back(forbidden_areas[i]["pan_min"]);
1111  min_pan_min_tilt_point.push_back(forbidden_areas[i]["tilt_min"]);
1112  min_pan_min_tilt_points.push_back(min_pan_min_tilt_point);
1113  forbidden_area_first_line_coordinate_forms.push_back(calculateCoordinateForm(max_pan_max_tilt_point, max_pan_min_tilt_point));
1114  forbidden_area_second_line_coordinate_forms.push_back(calculateCoordinateForm(max_pan_min_tilt_point, min_pan_min_tilt_point));
1115  forbidden_area_third_line_coordinate_forms.push_back(calculateCoordinateForm(min_pan_min_tilt_point, min_pan_max_tilt_point));
1116  forbidden_area_fourth_line_coordinate_forms.push_back(calculateCoordinateForm(min_pan_max_tilt_point, max_pan_max_tilt_point));
1117  }
1118 }
1119 
1129 }
1130 
1131 
1132 #ifdef __PTU_FREE_INCLUDED__
1133 long PTUDriver::get_current(char pan_or_tilt, char what) {
1134  if(pan_or_tilt == PAN) {
1135  switch(what) {
1136  case POSITION:
1138  break;
1139  case RESOLUTION:
1140  return (free_ptu.getPanResolution() * 60.0);
1141  break;
1142  case MINIMUM_POSITION:
1144  break;
1145  case MAXIMUM_POSITION:
1147  break;
1148  case SPEED:
1149  return free_ptu.getCurrentPanSpeed();
1150  break;
1151  case BASE:
1152  return free_ptu.getPanBaseSpeed();
1153  break;
1154  default:
1155  ROS_DEBUG("bad use of internal get_current method: input combination not legit");
1156  return ERROR;
1157  }
1158  }
1159  //Case TILT
1160  else if(pan_or_tilt == TILT) {
1161  switch(what) {
1162  case POSITION:
1164  break;
1165  case RESOLUTION:
1166  return (free_ptu.getTiltResolution() * 60);
1167  break;
1168  case MINIMUM_POSITION:
1170  break;
1171  case MAXIMUM_POSITION:
1173  break;
1174  case SPEED:
1175  return free_ptu.getCurrentTiltSpeed();
1176  break;
1177  case BASE:
1178  return free_ptu.getTiltBaseSpeed();
1179  break;
1180  default:
1181  ROS_DEBUG("bad use of internal get_current method: input combination not legit");
1182  return ERROR;
1183  }
1184  }
1185  else {
1186  ROS_DEBUG("bad use of internal get_current method: input combination not legit");
1187  return ERROR;
1188  }
1189 }
1190 
1191 //TYPE is atm almost never needed because only ABSOLUTE is used. If that changes, edit this method accordingly
1192 char PTUDriver::set_desired(char pan_or_tilt, char what, short int * value, char type) {
1193  if(pan_or_tilt == PAN) {
1194  switch(what) {
1195  case BASE:
1196  if(free_ptu.setPanBaseSpeed(*value)) {
1197  return PTU_OK;
1198  }
1199  else {
1200  return PTU_NOT_OK;
1201  }
1202  break;
1203  case UPPER_SPEED_LIMIT:
1205  return PTU_OK;
1206  }
1207  else {
1208  return PTU_NOT_OK;
1209  }
1210  break;
1211  case SPEED:
1212  if(free_ptu.setDesiredPanSpeedAbsolute(*value)) {
1213  return PTU_OK;
1214  }
1215  else {
1216  return PTU_NOT_OK;
1217  }
1218  break;
1219  case ACCELERATION:
1221  return PTU_OK;
1222  }
1223  else {
1224  return PTU_NOT_OK;
1225  }
1226  break;
1227  case HOLD_POWER_LEVEL:
1228  if(*value == PTU_OFF_POWER){
1230  return PTU_OK;
1231  }
1232  else {
1233  return PTU_NOT_OK;
1234  }
1235  }
1236  else if (*value == PTU_LOW_POWER){
1238  return PTU_OK;
1239  }
1240  else {
1241  return PTU_NOT_OK;
1242  }
1243  }
1244  else if(*value == PTU_REG_POWER) {
1246  return PTU_OK;
1247  }
1248  else {
1249  return PTU_NOT_OK;
1250  }
1251  }
1252  else {
1253  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1254  return PTU_NOT_OK;
1255  }
1256  break;
1257  case MOVE_POWER_LEVEL:
1258  if(*value == PTU_LOW_POWER){
1260  return PTU_OK;
1261  }
1262  else {
1263  return PTU_NOT_OK;
1264  }
1265  }
1266  else if (*value == PTU_REG_POWER){
1268  return PTU_OK;
1269  }
1270  else {
1271  return PTU_NOT_OK;
1272  }
1273  }
1274  else if (*value == PTU_HI_POWER) {
1276  return PTU_OK;
1277  }
1278  else {
1279  return PTU_NOT_OK;
1280  }
1281  }
1282  else {
1283  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1284  return PTU_NOT_OK;
1285  }
1286  break;
1287  case POSITION:
1289  return PTU_OK;
1290  }
1291  else {
1292  return PTU_NOT_OK;
1293  }
1294  break;
1295  default:
1296  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1297  return PTU_NOT_OK;
1298  }
1299  }
1300  else if(pan_or_tilt == TILT) {
1301  switch(what) {
1302  case BASE:
1303  if(free_ptu.setTiltBaseSpeed(*value)) {
1304  return PTU_OK;
1305  }
1306  else {
1307  return PTU_NOT_OK;
1308  }
1309  break;
1310  case UPPER_SPEED_LIMIT:
1312  return PTU_OK;
1313  }
1314  else {
1315  return PTU_NOT_OK;
1316  }
1317  break;
1318  case SPEED:
1320  return PTU_OK;
1321  }
1322  else {
1323  return PTU_NOT_OK;
1324  }
1325  break;
1326  case ACCELERATION:
1328  return PTU_OK;
1329  }
1330  else {
1331  return PTU_NOT_OK;
1332  }
1333  break;
1334  case HOLD_POWER_LEVEL:
1335  if(*value == PTU_OFF_POWER){
1337  return PTU_OK;
1338  }
1339  else {
1340  return PTU_NOT_OK;
1341  }
1342  }
1343  else if (*value == PTU_LOW_POWER){
1345  return PTU_OK;
1346  }
1347  else {
1348  return PTU_NOT_OK;
1349  }
1350  }
1351  else if (*value == PTU_REG_POWER) {
1353  return PTU_OK;
1354  }
1355  else {
1356  return PTU_NOT_OK;
1357  }
1358  }
1359  else {
1360  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1361  return PTU_NOT_OK;
1362  }
1363  break;
1364  case MOVE_POWER_LEVEL:
1365  if(*value == PTU_LOW_POWER){
1367  return PTU_OK;
1368  }
1369  else {
1370  return PTU_NOT_OK;
1371  }
1372  }
1373  else if (*value == PTU_REG_POWER){
1375  return PTU_OK;
1376  }
1377  else {
1378  return PTU_NOT_OK;
1379  }
1380  }
1381  else if (*value == PTU_HI_POWER) {
1383  return PTU_OK;
1384  }
1385  else {
1386  return PTU_NOT_OK;
1387  }
1388  }
1389  else {
1390  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1391  return PTU_NOT_OK;
1392  }
1393  break;
1394  case POSITION:
1396  return PTU_OK;
1397  }
1398  else {
1399  return PTU_NOT_OK;
1400  }
1401  break;
1402  default:
1403  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1404  return PTU_NOT_OK;
1405  }
1406  }
1407  else {
1408  ROS_DEBUG("bad use of internal set_desired method: input combination not legit");
1409  return PTU_NOT_OK;
1410  }
1411 
1412 }
1413 
1414 
1415 long PTUDriver::get_desired(char pan_or_tilt, char what) {
1416  long pow_mode;
1417  if(pan_or_tilt == PAN) {
1418  switch(what) {
1419  case UPPER_SPEED_LIMIT:
1421  break;
1422  case SPEED:
1423  return free_ptu.getDesiredPanSpeed();
1424  break;
1425  case ACCELERATION:
1426  return free_ptu.getPanAcceleartion();
1427  break;
1428  case HOLD_POWER_LEVEL:
1429  pow_mode = free_ptu.getPanStationaryPowerMode();
1430  if(pow_mode == LOW_HOLD_POWER_MODE) {
1431  return PTU_LOW_POWER;
1432  }
1433  else if(pow_mode == OFF_HOLD_POWER_MODE) {
1434  return PTU_OFF_POWER;
1435  }
1436  else if(pow_mode == REGULAR_HOLD_POWER_MODE) {
1437  return PTU_REG_POWER;
1438  }
1439  else {
1440  return ERROR;
1441  }
1442  break;
1443  case MOVE_POWER_LEVEL:
1444  pow_mode = free_ptu.getPanInMotionPowerMode();
1445  if(pow_mode == LOW_MOVE_POWER_MODE) {
1446  return PTU_LOW_POWER;
1447  }
1448  else if(pow_mode == HIGH_MOVE_POWER_MODE) {
1449  return PTU_HI_POWER;
1450  }
1451  else if(pow_mode == REGULAR_MOVE_POWER_MODE) {
1452  return PTU_REG_POWER;
1453  }
1454  else {
1455  return ERROR;
1456  }
1457  break;
1458  case POSITION:
1460  break;
1461  case BASE:
1462  return free_ptu.getPanBaseSpeed();
1463  break;
1464  default:
1465  return ERROR;
1466  }
1467  }
1468  else if (pan_or_tilt == TILT) {
1469  switch(what) {
1470  case UPPER_SPEED_LIMIT:
1472  break;
1473  case SPEED:
1474  return free_ptu.getDesiredTiltSpeed();
1475  break;
1476  case ACCELERATION:
1477  return free_ptu.getTiltAcceleartion();
1478  break;
1479  case HOLD_POWER_LEVEL:
1480  pow_mode = free_ptu.getTiltStationaryPowerMode();
1481  if(pow_mode == LOW_HOLD_POWER_MODE) {
1482  return PTU_LOW_POWER;
1483  }
1484  else if(pow_mode == OFF_HOLD_POWER_MODE) {
1485  return PTU_OFF_POWER;
1486  }
1487  else if(pow_mode == REGULAR_HOLD_POWER_MODE) {
1488  return PTU_REG_POWER;
1489  }
1490  else {
1491  return ERROR;
1492  }
1493  break;
1494  case MOVE_POWER_LEVEL:
1495  pow_mode = free_ptu.getTiltInMotionPowerMode();
1496  if(pow_mode == LOW_MOVE_POWER_MODE) {
1497  return PTU_LOW_POWER;
1498  }
1499  else if(pow_mode == HIGH_MOVE_POWER_MODE) {
1500  return PTU_HI_POWER;
1501  }
1502  else if(pow_mode == REGULAR_MOVE_POWER_MODE) {
1503  return PTU_REG_POWER;
1504  }
1505  else {
1506  return ERROR;
1507  }
1508  break;
1509  case POSITION:
1511  break;
1512  case BASE:
1513  return free_ptu.getTiltBaseSpeed();
1514  break;
1515  default:
1516  return ERROR;
1517  }
1518  }
1519  else {
1520  ROS_DEBUG("bad use of internal get_desired method: input combination not legit");
1521  return ERROR;
1522  }
1523 }
1524 
1525 char PTUDriver::set_mode(char mode_type, char mode) {
1526  if(mode_type == POSITION_LIMITS_MODE) {
1527  switch(mode) {
1528  case OFF_MODE:
1530  return PTU_OK;
1531  }
1532  else {
1533  return PTU_NOT_OK;
1534  }
1535  break;
1536  default:
1537  return PTU_NOT_OK;
1538  }
1539  }
1540  else if (mode_type == SPEED_CONTROL_MODE) {
1541  switch(mode) {
1544  return PTU_OK;
1545  }
1546  else {
1547  return PTU_NOT_OK;
1548  }
1549  break;
1552  return PTU_OK;
1553  }
1554  else {
1555  return PTU_NOT_OK;
1556  }
1557  break;
1558  default:
1559  return PTU_NOT_OK;
1560  }
1561  }
1562  else {
1563  ROS_DEBUG("bad use of internal set_mode method: input combination not legit");
1564  return PTU_NOT_OK;
1565  }
1566 }
1567 
1568 #endif // __PTU_FREE_INCLUDED__
1569 
1570 }
d
virtual void setSpeedControlMode(bool speed_control_mode)
setSpeedControlMode Method to set and disable pure speed control mode
Definition: PTUDriver.cpp:523
long getCurrentPanSpeed()
getCurrentPanSpeed Method that queries the current pan speed
Definition: PTUFree.cpp:265
virtual bool isConnected()
isConnected Method to check if ptu is connected.
Definition: PTUDriver.cpp:69
double calculateCoveredDistance(double acceleration_time, double slew_speed_time, double decceleration_time, bool is_pan)
Definition: PTUDriver.cpp:840
bool setDesiredTiltUpperSpeedLimit(short int upper_speed_limit)
setDesiredTiltUpperSpeedLimit Method that sets the upper speed limit for tilt (position/second). WARNING: Takes extremly long or does not work on every ptu
Definition: PTUFree.cpp:444
double getVectorLength(std::vector< double > input_vector)
Definition: PTUDriver.cpp:1007
virtual bool reachedGoal()
reachedGoal Method to determine if the PTU has reached its goal.
Definition: PTUDriver.cpp:653
virtual bool isWithinPanTiltLimits(double pan, double tilt)
isWithinPanTiltLimits Method that checks if pan and tilt value are within the chosen pan and tilt lim...
Definition: PTUDriver.cpp:385
std::vector< std::vector< double > > min_pan_max_tilt_points
Definition: PTUDriver.h:303
bool setDesiredPanSpeedAbsolute(short int speed)
setDesiredPanSpeedAbsolute Method that sets absolute pan speed (position/second)
Definition: PTUFree.cpp:165
virtual double getDesiredAngle(char type)
getDesiredAngle Method to get the desired pan/tilt angle (where the ptu moves to) ...
Definition: PTUDriver.cpp:549
virtual std::vector< double > determineLegitEndPoint(double end_point_pan_candidate, double end_point_tilt_candidate)
determineLegitEndPoint Method to determine an end point which is not inside a forbidden area using pa...
Definition: PTUDriver.cpp:1026
#define MINIMUM_POSITION
Definition: PTUDriver.h:37
long getCurrentUsedMaximumTiltPositionLimit()
getCurrentUsedMaximumTiltPositionLimit Method to get the currently used maximum tilt position...
Definition: PTUFree.cpp:789
long getDesiredTiltSpeed()
getDesiredTiltSpeed Method that queries the desired tilt speed
Definition: PTUFree.cpp:569
double getTiltResolution()
getTiltResolution Method that queries the tilt resolution (seconds/arc per position). Divide by 3600 to get Degree.
Definition: PTUFree.cpp:543
bool setDesiredTiltPositionAbsolute(short int position)
setDesiredTiltPositionAbsolute Method that sets absolute tilt position
Definition: PTUFree.cpp:375
std::vector< double > calculateCoordinateForm(std::vector< double > start_point, std::vector< double > end_point)
Definition: PTUDriver.cpp:992
virtual void setComputationTolerance(double computation_tolerance)
setComputationTolerance Sets the tolerance value for errors due to the imprecise nature of float...
Definition: PTUDriver.cpp:78
long get_current(char pan_or_tilt, char what)
#define PTU_PURE_VELOCITY_SPEED_CONTROL_MODE
Definition: PTUDriver.h:44
virtual void setDistanceFactor(long distance_factor)
setDistanceFactor Method to set the factor which determines how much samples are going to be taken fo...
Definition: PTUDriver.cpp:82
#define OFF_HOLD_POWER_MODE
Definition: PTUFree.h:32
long get_desired(char pan_or_tilt, char what)
long getCurrentUsedMinimumPanPositionLimit()
getCurrentUsedMinimumPanPositionLimit Method to get the currently used minimum pan position...
Definition: PTUFree.cpp:753
long convertTiltFromAngleToPosition(double angle)
Definition: PTUDriver.cpp:581
#define LIMITS_DISABLED
Definition: PTUFree.h:17
virtual bool setAbsoluteAngles(double pan_angle, double tilt_angle, bool no_forbidden_area_check)
setAbsoluteAngles Method to set pan and tilt angle for the PTU
Definition: PTUDriver.cpp:453
virtual bool isInSpeedControlMode()
isInSpeedControlMode Method to determine if ptu is in pure speed control mode
Definition: PTUDriver.cpp:532
long getTiltAcceleartion()
getTiltAcceleartion Method that queries the tilt acceleration (no current or desired here) ...
Definition: PTUFree.cpp:582
long getPanInMotionPowerMode()
getPanInMotionPowerMode Method to get the move power mode for pan axis.
Definition: PTUFree.cpp:1264
#define HIGH_MOVE_POWER_MODE
Definition: PTUFree.h:33
bool isOpen()
isOpen Method to determine if used port is open or closed.
Definition: PTUFree.cpp:2401
double convertPanFromPositionToAngle(long position)
Definition: PTUDriver.cpp:577
long getCurrentTiltSpeed()
getCurrentTiltSpeed Method that queries the current tilt speed
Definition: PTUFree.cpp:501
bool setTiltBaseSpeed(short int base_speed)
setTiltBaseSpeed Method that sets the base speed for tilt (position/second)
Definition: PTUFree.cpp:473
long getTiltUpperSpeedLimit()
getTiltUpperSpeedLimit Method that queries the tilt upper speed limit WARNING: This Method consumes e...
Definition: PTUFree.cpp:517
#define POSITION_LIMITS_MODE
Definition: PTUDriver.h:28
#define INDEPENDENT_SPEED_MODE
Definition: PTUFree.h:24
#define PTU_INDEPENDENT_SPEED_CONTROL_MODE
Definition: PTUDriver.h:47
virtual bool setValuesOutOfLimitsButWithinMarginToLimit(double *pan, double *tilt, double margin)
setValuesOutOfLimitsButWithinMarginToLimit This method is used to set pan/tilt values that are slighl...
Definition: PTUDriver.cpp:400
long getCurrentUsedMinimumTiltPositionLimit()
getCurrentUsedMinimumTiltPositionLimit Method to get the currently used minimum tilt position...
Definition: PTUFree.cpp:777
virtual long getLimitAngle(char pan_or_tilt, char upper_or_lower)
!!!!!!!!!Müssen nachfolgende public sein? Nach erstellen von mock überprüfen!!!!!!!!!!! ...
Definition: PTUDriver.cpp:278
#define PTU_NOT_OK
Definition: PTUDriver.h:49
long getDesiredPanSpeed()
getDesiredPanSpeed Method that queries the desired pan speed
Definition: PTUFree.cpp:333
virtual void setSettings(int pan_base, int tilt_base, int pan_speed, int tilt_speed, int pan_upper, int tilt_upper, int pan_accel, int tilt_accel, int pan_hold, int tilt_hold, int pan_move, int tilt_move)
setSettings Method to configure various settings of the ptu
Definition: PTUDriver.cpp:87
std::vector< double > calculatePointOfIntersectionWithForbiddenAreas(std::vector< double > start_point, std::vector< double > end_point)
Definition: PTUDriver.cpp:875
bool setDesiredPanUpperSpeedLimit(short int upper_speed_limit)
setDesiredPanUpperSpeedLimit Method that sets the upper speed limit for pan (position/second). WARNING: Takes extremly long or does not work on every ptu
Definition: PTUFree.cpp:208
bool setNewSerialConnection(std::string port, int baud)
setNewSerialConnection Establishes a new connection via serial port specified by &#39;port&#39; to a target d...
Definition: PTUFree.cpp:27
#define POSITION
Definition: PTUDriver.h:40
bool setPanStationaryPowerMode(long mode)
setPanStationaryPowerMode Method to set the stationary power mode for pan axis.
Definition: PTUFree.cpp:1120
#define ROS_WARN(...)
virtual void setLimitAngles(double pan_min, double pan_max, double tilt_min, double tilt_max)
setSettings Method to configure limits for pan and tilt
Definition: PTUDriver.cpp:487
#define MOVE_POWER_LEVEL
Definition: PTUDriver.h:36
std::vector< std::vector< double > > forbidden_area_third_line_coordinate_forms
Definition: PTUDriver.h:298
ptu_free::PTUFree free_ptu
Definition: PTUDriver.h:343
#define PTU_LOW_POWER
Definition: PTUDriver.h:41
long getTiltStationaryPowerMode()
getTiltStationaryPowerMode Method to get the stationary power mode for tilt axis. ...
Definition: PTUFree.cpp:1192
#define RESOLUTION
Definition: PTUDriver.h:27
bool setDesiredPanAccelerationAbsolute(short int acceleration)
setDesiredPanAccelerationAbsolute Method that sets the absolute pan acceleration (position/second^2) ...
Definition: PTUFree.cpp:193
std::vector< std::vector< double > > max_pan_min_tilt_points
Definition: PTUDriver.h:302
void setValuesToBackupValues(int &pan_base, int &tilt_base, int &pan_speed, int &tilt_speed, int &pan_upper, int &tilt_upper, int &pan_accel, int &tilt_accel, int &pan_hold, int &tilt_hold, int &pan_move, int &tilt_move)
Definition: PTUDriver.cpp:187
std::vector< std::vector< double > > forbidden_area_fourth_line_coordinate_forms
Definition: PTUDriver.h:299
~PTUDriver()
~PTUDriver Do not use, necessary because of some compiler issues.
Definition: PTUDriver.cpp:64
#define PTU_OK
Definition: PTUDriver.h:48
bool setDesiredTiltAccelerationAbsolute(short int acceleration)
setDesiredTiltAccelerationAbsolute Method that sets the absolute tilt acceleration (position/second^2...
Definition: PTUFree.cpp:429
#define SPEED_CONTROL_MODE
Definition: PTUDriver.h:46
static short int POW_VAL_MOVE[3]
Definition: PTUDriver.h:285
bool setPanBaseSpeed(short int base_speed)
setPanBaseSpeed Method that sets the base speed for pan (position/second)
Definition: PTUFree.cpp:237
std::vector< std::vector< double > > forbidden_area_first_line_coordinate_forms
Definition: PTUDriver.h:296
virtual std::string getErrorString(char status_code)
Definition: PTUDriver.cpp:589
long getTiltInMotionPowerMode()
getTiltInMotionPowerMode Method to get the move power mode for tilt axis.
Definition: PTUFree.cpp:1284
bool setSpeedControlMode(long mode)
setSpeedControlMode Method to set the sped control mode of the ptu.
Definition: PTUFree.cpp:996
long getPanAcceleartion()
getPanAcceleartion Method that queries the pan acceleration (no current or desired here) ...
Definition: PTUFree.cpp:346
#define ROS_INFO(...)
virtual void setForbiddenAreas(std::vector< std::map< std::string, double > > forbidden_areas)
setForbiddenAreas Method to set all forbidden areas for the PTU. Former forbidden areas will be disca...
Definition: PTUDriver.cpp:308
#define SPEED
Definition: PTUDriver.h:33
std::vector< double > getAccelerationTimeAndSlewSpeedTime(double distance_in_steps, double base_speed, double acceleration, double slew_speed)
Definition: PTUDriver.cpp:687
long getPanUpperSpeedLimit()
getPanUpperSpeedLimit Method that queries the pan upper speed limit WARNING: This Method consumes eit...
Definition: PTUFree.cpp:281
long getTiltBaseSpeed()
getTiltBaseSpeed Returns the current tilt base speed
Definition: PTUFree.cpp:595
#define UPPER_SPEED_LIMIT
Definition: PTUDriver.h:32
long getDesiredPanPosition()
getDesiredPanPosition Method that queries the desired pan position
Definition: PTUFree.cpp:320
long getCurrentPanPosition()
getCurrentPanPosition Method that queries the current pan position
Definition: PTUFree.cpp:252
bool checkReturnCode(char return_code)
Definition: PTUDriver.cpp:257
virtual double getAngleSpeed(char type)
getAngleSpeed Method to get the current angle speed of an axisa (deg/s)
Definition: PTUDriver.cpp:561
#define ERROR
Definition: PTUFree.h:12
PTUDriver()
PTUDriver Do not use, needed for mock.
Definition: PTUDriver.cpp:60
double getPanResolution()
getPanResolution Method that queries the pan resolution (seconds/arc per position). Divide by 3600 to get Degree.
Definition: PTUFree.cpp:307
virtual double getCurrentAngle(char type)
getCurrentAngle Method to get the current pan/tilt angle
Definition: PTUDriver.cpp:536
std::vector< std::vector< double > > forbidden_area_second_line_coordinate_forms
Definition: PTUDriver.h:297
#define ROS_DEBUG_STREAM(args)
long getPanBaseSpeed()
getPanBaseSpeed Returns the current pan base speed
Definition: PTUFree.cpp:359
#define LOW_HOLD_POWER_MODE
Definition: PTUFree.h:31
static short int POW_VAL_HOLD[3]
Definition: PTUDriver.h:286
virtual bool hasHaltedAndReachedGoal()
hasHaltedAndReachedGoal Method to determine if the PTU has halted and reached its goal ...
Definition: PTUDriver.cpp:642
virtual void setLimitAnglesToHardwareConstraints()
setLimitAnglesToHardwareConstraints Method that sets the pan and tilt limits to the maximum values th...
Definition: PTUDriver.cpp:443
std::vector< std::vector< double > > min_pan_min_tilt_points
Definition: PTUDriver.h:304
#define HOLD_POWER_LEVEL
Definition: PTUDriver.h:35
#define ACCELERATION
Definition: PTUDriver.h:34
bool isOnLineSegmentBetweenTwoPoints(std::vector< double > start_point, std::vector< double > end_point, std::vector< double > line_coordinate_form, std::vector< double > point_to_check, double tolerance)
Definition: PTUDriver.cpp:950
double convertTiltFromPositionToAngle(long position)
Definition: PTUDriver.cpp:585
long getPanStationaryPowerMode()
getPanStationaryPowerMode Method to get the stationary power mode for pan axis.
Definition: PTUFree.cpp:1172
std::vector< double > checkForPossibleKollision(double new_pan_angle, double new_tilt_angle)
Definition: PTUDriver.cpp:1053
#define REGULAR_HOLD_POWER_MODE
Definition: PTUFree.h:30
#define LOW_MOVE_POWER_MODE
Definition: PTUFree.h:35
#define PTU_REG_POWER
Definition: PTUDriver.h:42
#define OFF_MODE
Definition: PTUDriver.h:29
#define MAXIMUM_POSITION
Definition: PTUDriver.h:38
long getCurrentUsedMaximumPanPositionLimit()
getCurrentUsedMaximumPanPositionLimit Method to get the currently used maximum pan position...
Definition: PTUFree.cpp:765
#define ABSOLUTE
Definition: PTUDriver.h:31
bool setPositionLimitEnforcementMode(long enable)
setPositionLimitEnforcementMode Method to set the position limit enforcement mode. Warning: USER_DEFINED_LIMITS_ENABLED does not work on older PTUs and setting of this value is not tested.
Definition: PTUFree.cpp:610
bool setDesiredPanPositionAbsolute(short int position)
setDesiredPanPositionAbsolute Method that sets absolute pan position
Definition: PTUFree.cpp:139
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
std::vector< double > solveSecondDegreePolynomial(double a, double b, double c)
Definition: PTUDriver.cpp:666
#define REGULAR_MOVE_POWER_MODE
Definition: PTUFree.h:34
virtual bool isInForbiddenArea(double pan_angle, double tilt_angle)
isInForbiddenArea method to determine if a pan + tilt value pair lies within a forbidden area...
Definition: PTUDriver.cpp:362
#define PTU_OFF_POWER
Definition: PTUDriver.h:45
char set_desired(char pan_or_tilt, char what, short int *value, char type)
#define TILT
Definition: PTUDriver.h:26
#define PURE_VELOCITY_CONTROL_MODE
Definition: PTUFree.h:25
bool setTiltStationaryPowerMode(long mode)
setTiltStationaryPowerMode Method to set the stationary power mode for tilt axis. ...
Definition: PTUFree.cpp:1146
long convertPanFromAngleToPosition(double angle)
Definition: PTUDriver.cpp:573
bool setTiltInMotionPowerMode(long mode)
setTiltInMotionPowerMode Method to set the move power mode for tilt axis.
Definition: PTUFree.cpp:1238
std::vector< std::map< std::string, double > > forbidden_areas
Definition: PTUDriver.h:261
std::vector< double > calculateIntersectionPoint(std::vector< double > first_line_coordiante_form, std::vector< double > second_line_coordiante_form)
Definition: PTUDriver.cpp:934
virtual void setAbsoluteAngleSpeeds(double pan_speed, double tilt_speed)
setAbsoluteAngleSpeeds Method to set desired speed for pan/tilt movement absolute in deg/s ...
Definition: PTUDriver.cpp:331
#define BASE
Definition: PTUDriver.h:30
#define ROS_ERROR(...)
virtual bool hasHalted()
hasHalted Method to determine if PTU movement has stopped
Definition: PTUDriver.cpp:646
#define PTU_HI_POWER
Definition: PTUDriver.h:43
std::vector< double > predictPositionInTime(std::vector< double > start_point, std::vector< double > end_point, double point_in_time)
Definition: PTUDriver.cpp:736
long getCurrentTiltPosition()
getCurrentTiltPosition Method that queries the current tilt position
Definition: PTUFree.cpp:488
std::vector< std::vector< double > > max_pan_max_tilt_points
Definition: PTUDriver.h:301
#define PAN
Definition: PTUDriver.h:25
bool setDesiredTiltSpeedAbsolute(short int speed)
setDesiredTiltSpeedAbsolute Method that sets absolute tilt speed (position/second) ...
Definition: PTUFree.cpp:401
bool setPanInMotionPowerMode(long mode)
setPanInMotionPowerMode Method to set the move power mode for pan axis.
Definition: PTUFree.cpp:1213
long getDesiredTiltPosition()
getDesiredTiltPosition Method that queries the desired tilt position
Definition: PTUFree.cpp:556
char set_mode(char mode_type, char mode)
#define ROS_DEBUG(...)


asr_flir_ptu_driver
Author(s): Valerij Wittenbeck, Joachim Gehrung, Pascal Meißner, Patrick Schlosser
autogenerated on Mon Feb 28 2022 21:41:05