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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019