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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018