MRPT  1.9.9
CPtuDPerception.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/core/bits_math.h>
14 #include <mrpt/system/os.h>
16 
17 #include <cstring>
18 #include <iostream>
19 #include <thread>
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::system;
25 
26 /*-------------------------------------------------------------
27  moveToAbsPos
28 -------------------------------------------------------------*/
29 
30 bool CPtuDPerception::moveToAbsPos(char axis, double nRad)
31 {
32  if (!radAsign(axis, 'P', nRad)) return false;
33 
34  return true;
35 }
36 
37 /*-------------------------------------------------------------
38  absPosQ
39 -------------------------------------------------------------*/
40 
41 bool CPtuDPerception::absPosQ(char axis, double& nRad)
42 {
43  return radQuerry(axis, 'P', nRad);
44 }
45 
46 /*-------------------------------------------------------------
47  moveToOffPos
48 -------------------------------------------------------------*/
49 
50 bool CPtuDPerception::moveToOffPos(char axis, double nRad)
51 {
52  if (!radAsign(axis, 'O', nRad)) return false;
53 
54  return true;
55 }
56 
57 /*-------------------------------------------------------------
58  offPosQ
59 -------------------------------------------------------------*/
60 
61 bool CPtuDPerception::offPosQ(char axis, double& nRad)
62 {
63  return radQuerry(axis, 'O', nRad);
64 }
65 
66 /*-------------------------------------------------------------
67  maxPosQ
68 -------------------------------------------------------------*/
69 
70 bool CPtuDPerception::maxPosQ(char axis, double& nRad)
71 {
72  return radQuerry(axis, 'X', nRad);
73 }
74 
75 /*-------------------------------------------------------------
76  minPosQ
77 -------------------------------------------------------------*/
78 
79 bool CPtuDPerception::minPosQ(char axis, double& nRad)
80 {
81  return radQuerry(axis, 'N', nRad);
82 }
83 
84 /*-------------------------------------------------------------
85  speed
86 -------------------------------------------------------------*/
87 
88 bool CPtuDPerception::speed(char axis, double radSec)
89 {
90  return radAsign(axis, 'S', radSec);
91 }
92 
93 /*-------------------------------------------------------------
94  speedQ
95 -------------------------------------------------------------*/
96 
97 bool CPtuDPerception::speedQ(char axis, double& radSec)
98 {
99  return radQuerry(axis, 'S', radSec);
100 }
101 
102 /*-------------------------------------------------------------
103  aceleration
104 -------------------------------------------------------------*/
105 
106 bool CPtuDPerception::aceleration(char axis, double radSec2)
107 {
108  return radAsign(axis, 'A', radSec2);
109 }
110 
111 /*-------------------------------------------------------------
112  acelerationQ
113 -------------------------------------------------------------*/
114 
115 bool CPtuDPerception::acelerationQ(char axis, double& radSec2)
116 {
117  return radQuerry(axis, 'A', radSec2);
118 }
119 
120 /*-------------------------------------------------------------
121  baseSpeed
122 -------------------------------------------------------------*/
123 
124 bool CPtuDPerception::baseSpeed(char axis, double radSec)
125 {
126  return radAsign(axis, 'B', radSec);
127 }
128 
129 /*-------------------------------------------------------------
130  baseSpeedQ
131 -------------------------------------------------------------*/
132 
133 bool CPtuDPerception::baseSpeedQ(char axis, double& radSec)
134 {
135  return radQuerry(axis, 'B', radSec);
136 }
137 
138 /*-------------------------------------------------------------
139  upperSpeed
140 -------------------------------------------------------------*/
141 
142 bool CPtuDPerception::upperSpeed(char axis, double radSec)
143 {
144  return radAsign(axis, 'U', radSec);
145 }
146 
147 /*-------------------------------------------------------------
148  upperSpeedQ
149 -------------------------------------------------------------*/
150 
151 bool CPtuDPerception::upperSpeedQ(char axis, double& radSec)
152 {
153  return radQuerry(axis, 'U', radSec);
154 }
155 
156 /*-------------------------------------------------------------
157  lowerSpeed
158 -------------------------------------------------------------*/
159 
160 bool CPtuDPerception::lowerSpeed(char axis, double radSec)
161 {
162  return radAsign(axis, 'L', radSec);
163 }
164 
165 /*-------------------------------------------------------------
166  lowerSpeedQ
167 -------------------------------------------------------------*/
168 
169 bool CPtuDPerception::lowerSpeedQ(char axis, double& radSec)
170 {
171  return radQuerry(axis, 'L', radSec);
172 }
173 
174 /*-------------------------------------------------------------
175  enableLimitsQ
176 -------------------------------------------------------------*/
177 
178 bool CPtuDPerception::enableLimitsQ(bool& enable)
179 {
180  char response[150];
181 
182  if (!transmit("L") || !receive("L", response)) return false;
183 
184  if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr)
185  enable = true;
186  else
187  enable = false;
188 
189  return true;
190 }
191 
192 /*-------------------------------------------------------------
193  enableLimits
194 -------------------------------------------------------------*/
195 
196 bool CPtuDPerception::enableLimits(bool set)
197 {
198  if (set)
199  return (transmit("LE") && receive("LE", nullptr));
200  else
201  return (transmit("LD") && receive("LD", nullptr));
202 }
203 
204 /*-------------------------------------------------------------
205  inmediateExecution
206 -------------------------------------------------------------*/
207 
208 bool CPtuDPerception::inmediateExecution(bool set)
209 {
210  if (set)
211  return (transmit("I") && receive("I", nullptr));
212  else
213  return (transmit("S") && receive("S", nullptr));
214 }
215 
216 /*-------------------------------------------------------------
217  aWait
218 -------------------------------------------------------------*/
219 
220 bool CPtuDPerception::aWait()
221 {
222  return (transmit("A") && receive("A", nullptr));
223 }
224 
225 /*-------------------------------------------------------------
226  haltAll
227 -------------------------------------------------------------*/
228 
229 bool CPtuDPerception::haltAll()
230 {
231  return (transmit("H") && receive("H", nullptr));
232 }
233 
234 /*-------------------------------------------------------------
235  halt
236 -------------------------------------------------------------*/
237 
238 bool CPtuDPerception::halt(char axis)
239 {
240  char sTrans[3];
241  sTrans[0] = 'H';
242  sTrans[1] = axis;
243  sTrans[2] = '\0';
244 
245  return (transmit(sTrans) && receive(sTrans, nullptr));
246 }
247 
248 /*-------------------------------------------------------------
249  reset
250 -------------------------------------------------------------*/
251 
252 bool CPtuDPerception::reset()
253 {
254  if (!transmit("R")) return false;
255  receive("R", nullptr);
256 
257  return panTiltHitError();
258 }
259 
260 /*-------------------------------------------------------------
261  save
262 -------------------------------------------------------------*/
263 
264 bool CPtuDPerception::save()
265 {
266  return (transmit("DS") && receive("DS", nullptr));
267 }
268 
269 /*-------------------------------------------------------------
270  restoreDefaults
271 -------------------------------------------------------------*/
272 
273 bool CPtuDPerception::restoreDefaults()
274 {
275  return (transmit("DR") && receive("DR", nullptr));
276 }
277 
278 /*-------------------------------------------------------------
279  restoreFactoryDefaults
280 -------------------------------------------------------------*/
281 
282 bool CPtuDPerception::restoreFactoryDefaults()
283 {
284  return (transmit("DF") && receive("DF", nullptr));
285 }
286 
287 /*-------------------------------------------------------------
288  version
289 -------------------------------------------------------------*/
290 
291 bool CPtuDPerception::version(char* sVersion)
292 {
293  return (transmit("V") && receive("V", sVersion));
294 }
295 
296 /*-------------------------------------------------------------
297  powerModeQ
298 -------------------------------------------------------------*/
299 
300 bool CPtuDPerception::powerModeQ(bool transit, char& mode)
301 {
302  const char* response = "";
303 
304  if (transit)
305  {
306  if (!transmit("PM"))
307  return false;
308  else if (!transmit("PH"))
309  return false;
310  }
311 
312  if (strstr(upperCase(response).c_str(), "REGULAR") != nullptr)
313  mode = Regular;
314  else if (strstr(upperCase(response).c_str(), "HIGH") != nullptr)
315  mode = High;
316  else if (strstr(upperCase(response).c_str(), "LOW") != nullptr)
317  mode = Low;
318  else // OFF
319  mode = Off;
320 
321  return true;
322 }
323 
324 /*-------------------------------------------------------------
325  powerMode
326 -------------------------------------------------------------*/
327 
328 bool CPtuDPerception::powerMode(bool transit, char mode)
329 {
330  char sTrans[4]; //="";
331  sTrans[0] = 'P';
332  sTrans[1] = transit ? 'M' : 'H';
333  sTrans[2] = mode;
334  sTrans[3] = '\0';
335 
336  return (transmit(sTrans) && receive(sTrans, nullptr));
337 }
338 
339 /*-------------------------------------------------------------
340  init
341 -------------------------------------------------------------*/
342 
343 bool CPtuDPerception::init(const string& port)
344 {
345  try
346  {
347  serPort.open(port);
348 
349  cout << endl << "[INFO] Start PTU communication config:" << endl;
350 
351  cout << "[PTU::OpenSerialPort] Opening serial port...";
352 
353  if (serPort.isOpen())
354  {
355  }
356  else
357  {
358  cout << " Error opening serial port";
359  return false;
360  }
361 
362  cout << "OK" << endl;
363 
364  cout << "[PTU::SetTimeouts] Setting timeouts...";
365  serPort.setTimeouts(1000, 1, 1000, 1, 1000);
366  cout << "OK" << endl;
367 
368  cout << "[PTU::setBaudRate] Setting baud rate...";
369  serPort.setConfig(9600);
370  cout << "OK" << endl;
371 
372  // PTU initial configuration
373  cout << "[PTU::setInitialConfiguration] Setting initial "
374  "configuration...";
375  if ((!verbose(true)) || // Original: false Actual: true
376  (!resolution()) || (!echoMode(true)) || (!inmediateExecution(true)))
377  {
378  cout << " Error setting initial configuration";
379  serPort.close();
380  return false;
381  }
382 
383  cout << "OK" << endl
384  << endl
385  << "[INFO] Pan Resolution: " << panResolution << " radians | "
386  << RAD2DEG(panResolution) << "degrees";
387  cout << endl
388  << "[INFO] TitlResolution: " << tiltResolution << " radians | "
389  << RAD2DEG(tiltResolution) << "degrees" << endl
390  << endl;
391  }
392  catch (exception& e)
393  {
394  cerr << e.what() << endl;
395  return false;
396  }
397 
398  return true;
399 }
400 
401 /*-------------------------------------------------------------
402  close
403 -------------------------------------------------------------*/
404 
405 void CPtuDPerception::close() { serPort.close(); }
406 /*-------------------------------------------------------------
407  radError
408 -------------------------------------------------------------*/
409 
410 double CPtuDPerception::radError(char axis, double nRadMoved)
411 {
412  double div;
413 
414  if (axis == Pan)
415  div = nRadMoved - long(nRadMoved / panResolution) * panResolution;
416  else
417  div = nRadMoved - long(nRadMoved / tiltResolution) * tiltResolution;
418 
419  return div;
420 }
421 
422 /*-------------------------------------------------------------
423  transmit
424 -------------------------------------------------------------*/
425 
426 bool CPtuDPerception::transmit(const char* command)
427 {
428  char str[20] = "";
429 
430  strcpy(str, command);
431  strcat(str, " ");
432 
433  size_t written = serPort.Write(str, strlen(str));
434 
435  if (!written)
436  {
437  return false;
438  }
439 
440  return true;
441 }
442 
443 /*-------------------------------------------------------------
444  receive
445 -------------------------------------------------------------*/
446 
447 bool CPtuDPerception::receive(const char* command, char* response)
448 {
449  int cnt = 0;
450  unsigned long nReaden;
451  char str[150] = "";
452  char* tmp;
453 
454  do
455  {
456  nReaden = serPort.Read(&str[cnt], 1);
457  if (nReaden != 0) cnt++;
458  } while ((nReaden != 0) && (((tmp = strstr(str, command)) == nullptr) ||
459  (str[cnt - 1] != '\n')));
460 
461  if (nReaden == 0)
462  {
463  nError = nError * TimeoutError;
464  return false;
465  }
466 
467  if (response != nullptr)
468  {
469  //*response=new char[150];
470  strcpy(response, tmp);
471  }
472 
473  // cout << str << endl;
474 
475  if (strstr(tmp, "!") == nullptr)
476  {
477  nError = nError * NoError;
478  return true;
479  }
480 
481  if ((strstr(tmp, "!P") != nullptr) && (strstr(tmp, "!T") != nullptr))
482  nError = nError * PanTiltHitError;
483  else if (strstr(tmp, "!T") != nullptr)
484  nError = nError * TiltHitError;
485  else if (strstr(tmp, "!P") != nullptr)
486  nError = nError * PanHitError;
487  else if (strstr(tmp, "! Maximum") != nullptr)
488  nError = nError * MaxLimitError;
489  else if (strstr(tmp, "! Minimum") != nullptr)
490  nError = nError * MinLimitError;
491  else if (strstr(tmp, "! Value") != nullptr)
492  nError = nError * OutOfRange;
493  else if (strstr(tmp, "! Illegal") != nullptr)
494  nError = nError * IllegalCommandError;
495  else
496  nError = nError * UnExpectedError;
497 
498  return false;
499 }
500 
501 /*-------------------------------------------------------------
502  verboseQ
503 -------------------------------------------------------------*/
504 
505 bool CPtuDPerception::verboseQ(bool& mode)
506 {
507  char response[150];
508 
509  if (!transmit("F") || !receive("F", response)) return false;
510 
511  if (strstr(response, "VERBOSE") != nullptr)
512  mode = true;
513  else
514  mode = false;
515 
516  return true;
517 }
518 
519 /*-------------------------------------------------------------
520  verbose
521 -------------------------------------------------------------*/
522 
524 {
525  if (set)
526  return (transmit("FV") && (receive("FV", nullptr)));
527  else
528  return (transmit("FT") && (receive("FT", nullptr)));
529 }
530 
531 /*-------------------------------------------------------------
532  echoModeQ
533 -------------------------------------------------------------*/
534 
535 bool CPtuDPerception::echoModeQ(bool& mode)
536 {
537  char response[150];
538 
539  if (!transmit("E") || !receive("E", response)) return false;
540 
541  if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr)
542  mode = true;
543  else
544  mode = false;
545 
546  return true;
547 }
548 
549 /*-------------------------------------------------------------
550  echoMode
551 -------------------------------------------------------------*/
552 
553 bool CPtuDPerception::echoMode(bool mode)
554 {
555  if (mode)
556  return (transmit("EE") && receive("EE", nullptr));
557  else
558  return (transmit("ED") && receive("ED", nullptr));
559 }
560 
561 /*-------------------------------------------------------------
562  resolution
563 -------------------------------------------------------------*/
564 
565 bool CPtuDPerception::resolution()
566 {
567  char response[150];
568 
569  if ((!transmit("PR")) || (!receive("PR", response))) return false;
570  panResolution = DEG2RAD(convertToDouble(response) / 3600);
571 
572  if ((!transmit("TR")) || (!receive("TR", response))) return false;
573  tiltResolution = DEG2RAD(convertToDouble(response) / 3600);
574 
575  return true;
576 }
577 
578 /*-------------------------------------------------------------
579  radQuerry
580 -------------------------------------------------------------*/
581 
582 bool CPtuDPerception::radQuerry(char axis, char command, double& rad)
583 {
584  char response[150];
585  char sTrans[3];
586 
587  sTrans[0] = axis;
588  sTrans[1] = command;
589  sTrans[2] = '\0';
590 
591  if ((!transmit(sTrans)) || (!receive(sTrans, response))) return false;
592 
593  rad = posToRad(axis, convertToLong(response));
594 
595  return true;
596 }
597 
598 /*-------------------------------------------------------------
599  radAsign
600 -------------------------------------------------------------*/
601 
602 bool CPtuDPerception::radAsign(char axis, char command, double nRad)
603 {
604  char sPos[20];
605  char sTrans[22];
606 
607  char response[150];
608 
609  os::sprintf(sPos, sizeof(sPos), "%li", radToPos(axis, nRad));
610 
611  sTrans[0] = axis;
612  sTrans[1] = command;
613  strcpy(&sTrans[2], sPos);
614 
615  return (transmit(sTrans) && receive(sTrans, response));
616 }
617 
618 /*-------------------------------------------------------------
619  scan
620 -------------------------------------------------------------*/
621 
622 bool CPtuDPerception::scan(
623  char axis, int tWait, float initial, float final, double radPre)
624 {
625  // Check initial and final positions
626  if (initial < final)
627  {
628  float aux = initial;
629  initial = final;
630  final = aux;
631  }
632 
633  // Go to initial position
634  moveToAbsPos(axis, initial);
635  aWait();
636 
637  std::this_thread::sleep_for(500ms);
638 
639  double j = 0;
640  offPosQ(axis, j);
641 
642  long steps = radToPos(axis, radPre);
643  long totalSteps;
644 
645  // Obtain total number of steps
646  int initialPos = radToPos(axis, initial);
647  int finalPos = radToPos(axis, final);
648 
649  totalSteps = std::abs(initialPos - finalPos);
650 
651  // Performs first sweep
652  for (int i = 0; i < totalSteps / steps; i++)
653  {
654  if (initial > final)
655  {
656  moveToOffPos(axis, -radPre);
657  }
658  else
659  {
660  moveToOffPos(axis, radPre);
661  }
662  offPosQ(axis, j);
663  std::this_thread::sleep_for(std::chrono::milliseconds(tWait));
664  }
665 
666  // Adjust steps for second scan
667  moveToOffPos(axis, radPre / 2);
668  aWait();
669 
670  // Performs seecond scan
671  for (int i = 0; i < (totalSteps / steps) - 1; i++)
672  {
673  if (initial > final)
674  {
675  moveToOffPos(axis, radPre);
676  }
677  else
678  {
679  moveToOffPos(axis, -radPre);
680  }
681  offPosQ(axis, j);
682  std::this_thread::sleep_for(std::chrono::milliseconds(tWait));
683  }
684 
685  offPosQ(axis, j);
686 
687  // Return to initial position
688  moveToAbsPos(axis, 0);
689 
690  return true;
691 }
692 
693 /*-------------------------------------------------------------
694  radToPos
695 -------------------------------------------------------------*/
696 
697 long CPtuDPerception::radToPos(char axis, double nrad)
698 {
699  if (axis == Pan)
700  return (long)(nrad / panResolution);
701  else
702  return (long)(nrad / tiltResolution);
703 }
704 
705 /*-------------------------------------------------------------
706  posToRad
707 -------------------------------------------------------------*/
708 
709 double CPtuDPerception::posToRad(char axis, long nPos)
710 {
711  if (axis == Pan)
712  return (double)nPos * panResolution;
713  else
714  return (double)nPos * tiltResolution;
715 }
716 
717 /*-------------------------------------------------------------
718  convertToLong
719 -------------------------------------------------------------*/
720 
722 {
723  char* result = strpbrk(sLong, "-0123456789");
724  char* stop;
725 
726  return strtol(result, &stop, 10);
727 }
728 
729 /*-------------------------------------------------------------
730  convertToDouble
731 -------------------------------------------------------------*/
732 
733 double CPtuDPerception::convertToDouble(char* sDouble)
734 {
735  char* result = strpbrk(sDouble, "-0123456789");
736  char* stop;
737 
738  return strtod(result, &stop);
739 }
740 
741 /*-------------------------------------------------------------
742  checkError
743 -------------------------------------------------------------*/
744 
745 int CPtuDPerception::checkErrors()
746 {
747  int code = 0;
748 
749  // Check for errors
750  if (noError())
751  {
752  code = 0;
753  }
754  else
755  {
756  if (comError())
757  {
758  code = 1;
759  }
760  else if (timeoutError())
761  {
762  code = 2;
763  }
764  else if (initError())
765  {
766  code = 3;
767  }
768  else if (panTiltHitError())
769  {
770  code = 4;
771  }
772  else if (panHitError())
773  {
774  code = 5;
775  }
776  else if (tiltHitError())
777  {
778  code = 6;
779  }
780  else if (maxLimitError())
781  {
782  code = 7;
783  }
784  else if (minLimitError())
785  {
786  code = 8;
787  }
788  else if (outOfRange())
789  {
790  code = 9;
791  }
792  else if (illegalCommandError())
793  {
794  code = 10;
795  }
796  else if (unExpectedError())
797  {
798  code = 11;
799  }
800  }
801 
802  return code;
803 }
804 
805 /*-------------------------------------------------------------
806  nVersion
807 -------------------------------------------------------------*/
808 
809 void CPtuDPerception::nversion(double& nVersion)
810 {
811  cout << "[ERROR] Function not defined for this PTU model";
812  nVersion = 0;
813 }
814 
815 /*-------------------------------------------------------------
816  setLimits
817 -------------------------------------------------------------*/
818 
819 bool CPtuDPerception::setLimits(
820  [[maybe_unused]] char axis, [[maybe_unused]] double& l,
821  [[maybe_unused]] double& u)
822 {
823  cout << "[ERROR] Function not defined for this PTU model";
824  return false;
825 }
826 
827 /*-------------------------------------------------------------
828  changeMotionDir
829 -------------------------------------------------------------*/
830 
831 bool CPtuDPerception::changeMotionDir()
832 {
833  cout << "[ERROR] Function not defined for this PTU model";
834  return false;
835 }
836 
837 /*-------------------------------------------------------------
838  rangeMeasure
839 -------------------------------------------------------------*/
840 
841 bool CPtuDPerception::rangeMeasure()
842 {
843  cout << "[ERROR] Function not defined for this PTU model";
844  return false;
845 }
char * strcat(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcat.
Contains classes for various device interfaces.
STL namespace.
char * strcpy(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcpy.
long convertToLong(char *sLong)
Definition: CRovio.cpp:396
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020