p2os_ptz.cpp
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2000
4  * Tucker Hermans
5  *
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20  *
21  */
22 
23 #include <p2os_driver/p2os_ptz.h>
24 #include <p2os_driver/p2os.h>
25 
26 //
27 // Constants
28 //
29 const int P2OSPtz::MAX_COMMAND_LENGTH = 19;
30 const int P2OSPtz::MAX_REQUEST_LENGTH = 17;
32 const int P2OSPtz::PACKET_TIMEOUT = 300;
33 const int P2OSPtz::SLEEP_TIME_USEC = 300000;
34 const int P2OSPtz::PAN_THRESH = 1;
35 const int P2OSPtz::TILT_THRESH = 1;
36 const int P2OSPtz::ZOOM_THRESH = 1;
37 
38 //
39 // Constructors
40 //
41 P2OSPtz::P2OSPtz(P2OSNode * p2os, bool bidirectional_com) :
42  p2os_(p2os), max_zoom_(MAX_ZOOM_OPTIC), pan_(0), tilt_(0), zoom_(0),
43  is_on_(false), error_code_(CAM_ERROR_NONE),
44  bidirectional_com_(bidirectional_com)
45 {
46  current_state_.pan = 0;
47  current_state_.zoom = 0;
48  current_state_.tilt = 0;
49  current_state_.relative = false;
50 }
51 
52 
53 //
54 // Core Functions
55 //
57 {
58  int err = 0;
59  int num_inits = 7;
60  is_on_ = true;
61  for (int i = 1; i < num_inits; i++) {
62  switch(i)
63  {
64  // case 0:
65  // do
66  // {
67  // ROS_DEBUG("Waiting for camera to power off.");
68  // err = setPower(POWER_OFF);
69  // } while (error_code_ == CAM_ERROR_BUSY);
70  // break;
71  case 1:
72  do
73  {
74  ROS_DEBUG("Waiting for camera to power on.");
75  err = setPower(POWER_ON);
76  } while (error_code_ == CAM_ERROR_BUSY);
77  break;
78  case 2:
79  do
80  {
81  ROS_DEBUG("Waiting for camera mode to set");
82  err = setControlMode();
83  } while (error_code_ == CAM_ERROR_BUSY);
84  break;
85  case 3:
86  do
87  {
88  ROS_DEBUG("Waiting for camera to initialize");
89  err = sendInit();
90  } while (error_code_ == CAM_ERROR_BUSY);
91  break;
92  case 4:
93  do
94  {
95  for(int i = 0; i < 3; i++)
96  {
97  ROS_DEBUG("Waiting for camera to set default tilt");
98  err = setDefaultTiltRange();
99  }
100  } while (error_code_ == CAM_ERROR_BUSY);
101  break;
102  case 5:
103  do
104  {
105  ROS_DEBUG("Waiting for camera to set initial pan and tilt");
106  err = sendAbsPanTilt(0, 0);
107  } while (error_code_ == CAM_ERROR_BUSY);
108  break;
109  case 6:
110  do
111  {
112  ROS_DEBUG("Waiting for camera to set initial zoom");
113  err = sendAbsZoom(0);
114  } while (error_code_ == CAM_ERROR_BUSY);
115  break;
116  default:
117  err = -7;
118  break;
119  }
120 
121  // Check for erros after each attempt
122  if (err)
123  {
124  ROS_ERROR("Error initiliazing PTZ at stage %i", i);
125  switch(error_code_)
126  {
127  case CAM_ERROR_BUSY:
128  ROS_ERROR("Error: CAM_ERROR_BUSY");
129  break;
130  case CAM_ERROR_PARAM:
131  ROS_ERROR("Error: CAM_ERROR_PARAM");
132  break;
133  case CAM_ERROR_MODE:
134  ROS_ERROR("Error: CAM_ERROR_MODE");
135  break;
136  default:
137  ROS_ERROR("Error: Unknown error response from camera.");
138  break;
139  }
140  return(-1);
141  }
142  else
143  {
144  ROS_DEBUG("Passed stage %i of PTZ initialization.", i);
145  }
146  }
147  ROS_DEBUG("Finished initialization of the PTZ.");
148  return 0;
149 }
150 
152 {
153  sendAbsPanTilt(0,0);
154  usleep(SLEEP_TIME_USEC);
155  sendAbsZoom(0);
156  usleep(SLEEP_TIME_USEC);
158  usleep(SLEEP_TIME_USEC);
159  ROS_INFO("PTZ camera has been shutdown");
160 }
161 
162 void P2OSPtz::callback(const p2os_msgs::PTZStateConstPtr &cmd)
163 {
164  p2os_msgs::PTZState to_send;
165  bool change_pan_tilt = false;
166  bool change_zoom = false;
167  to_send.pan = pan_;
168  to_send.tilt = tilt_;
169  to_send.zoom = zoom_;
170 
171  // Check if the command is relative to the current position
172  if (cmd->relative)
173  {
174  if ( abs(cmd->pan) > PAN_THRESH)
175  {
176  to_send.pan = cmd->pan + pan_;
177  change_pan_tilt = true;
178  }
179  if ( abs(cmd->tilt) > TILT_THRESH)
180  {
181  to_send.tilt = cmd->tilt + tilt_;
182  change_pan_tilt = true;
183  }
184  if ( abs(cmd->zoom) > ZOOM_THRESH)
185  {
186  to_send.zoom = cmd->zoom + zoom_;
187  change_zoom = true;
188  }
189  }
190  else
191  {
192  if ( abs(cmd->pan - pan_) > PAN_THRESH)
193  {
194  to_send.pan = cmd->pan;
195  change_pan_tilt = true;
196  }
197  if ( abs(cmd->tilt - tilt_) > TILT_THRESH)
198  {
199  to_send.tilt = cmd->tilt;
200  change_pan_tilt = true;
201  }
202  if ( abs(cmd->zoom - zoom_) > ZOOM_THRESH)
203  {
204  to_send.zoom = cmd->zoom;
205  change_zoom = true;
206  }
207  }
208 
209  if (change_pan_tilt)
210  {
211  sendAbsPanTilt(to_send.pan, to_send.tilt);
212  }
213  if (change_zoom)
214  {
215  sendAbsZoom(to_send.zoom);
216  }
217 
218  current_state_.pan = pan_;
219  current_state_.zoom = zoom_;
220  current_state_.tilt = tilt_;
221 }
222 
223 //
224 // Communication Functions
225 //
226 int P2OSPtz::sendCommand(unsigned char *str, int len)
227 {
228  P2OSPacket ptz_packet;
229  P2OSPacket request_pkt;
230  unsigned char request[4];
231 
232  // Zero out the Receive Buffer
233  request[0] = GETAUX;
234  request[1] = ARGINT;
235  request[2] = 0;
236  request[3] = 0;
237  request_pkt.Build(request,4);
238  p2os_->SendReceive(&request_pkt,false);
239 
240  if(len > MAX_COMMAND_LENGTH)
241  {
242  ROS_ERROR("Command message is too large to send");
243  return(-1);
244  }
245 
246  // Since I'm hardcoding this to AUX1, basically we gotta stick the AUX1DATA
247  // header on this and then give it to the p2os send command.
248  unsigned char mybuf[MAX_COMMAND_LENGTH+3];
249  mybuf[0] = TTY2;
250  mybuf[1] = ARGSTR;
251  mybuf[2] = len;
252  // Copy the command
253  memcpy(&mybuf[3], str, len);
254  ptz_packet.Build(mybuf, len+3);
255 
256  // Send the packet
257  p2os_->SendReceive(&ptz_packet, false);
258 
259  return(0);
260 }
261 
262 int P2OSPtz::sendRequest(unsigned char *str, int len, unsigned char *reply)
263 {
264  P2OSPacket ptz_packet;
265  P2OSPacket request_pkt;
266  unsigned char request[4];
267 
268  // Zero out the Receive Buffer
269  request[0] = GETAUX;
270  request[1] = ARGINT;
271  request[2] = 0;
272  request[3] = 0;
273  request_pkt.Build(request,4);
274  p2os_->SendReceive(&request_pkt,false);
275 
276  if (len > MAX_REQUEST_LENGTH)
277  {
278  ROS_ERROR("Request message is too large to send.");
279  return -1;
280  }
281 
282  // Since I'm hardcoding this to AUX1, basically we gotta stick the AUX1DATA
283  // header on this and then give it to the p2os send command.
284  unsigned char mybuf[MAX_REQUEST_LENGTH];
285  mybuf[0] = TTY2;
286  mybuf[1] = ARGSTR;
287  mybuf[2] = len;
288  // Copy the command
289  memcpy(&mybuf[3], str, len);
290  ptz_packet.Build(mybuf, len+3);
291 
292  // Send the packet
293  p2os_->SendReceive(&ptz_packet, false);
294 
295 
296  return 0;
297 }
298 
300 {
301  int num;
302  unsigned char reply[MAX_REQUEST_LENGTH];
303  int len = 0;
304  unsigned char byte;
305  int t;
306  memset(reply, 0, COMMAND_RESPONSE_BYTES);
307 
308  getPtzPacket(asize);
309 
310  for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
311  {
312  // if we don't get any bytes, or if we've just exceeded the limit
313  // then return null
314  t = cb_.getFromBuf();
315  if ( t < 0 )
316  {
317  // Buf Error!
318  ROS_ERROR("circbuf error!");
319  return -1;
320  }
321  else
322  {
323  byte = (unsigned char)t;
324  }
325  if (byte == (unsigned char)RESPONSE)
326  {
327  reply[0] = byte;
328  len ++;
329  break;
330  }
331  }
332 
333  if (len == 0)
334  {
335  ROS_ERROR("Length is 0 on received packet.");
336  return -1;
337  }
338 
339  // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
340  for(num = 1; num <= MAX_REQUEST_LENGTH; num++)
341  {
342  t = cb_.getFromBuf();
343  if (t < 0)
344  {
345  // there are no more bytes, so check the last byte for the footer
346  if (reply[len - 1] != (unsigned char)FOOTER)
347  {
348  ROS_ERROR("canonvcc4::receiveCommandAnswer: Discarding bad packet.");
349  return -1;
350  }
351  else
352  break;
353  }
354  else
355  {
356  // add the byte to the array
357  reply[len] = (unsigned char)t;
358  len ++;
359  }
360  }
361 
362  // Check the response
363  if (len != COMMAND_RESPONSE_BYTES)
364  {
365  ROS_ERROR("Answer does not equal command response bytes");
366  return -1;
367  }
368 
369  // check the header and footer
370  if (reply[0] != (unsigned char)RESPONSE || reply[5] != (unsigned char)FOOTER)
371  {
372  ROS_ERROR("Header or Footer is wrong on received packet");
373  return -1;
374  }
375 
376  // so far so good. Set myError to the error byte
377  error_code_ = reply[3];
379  {
380  return 0;
381  }
382 
383  switch(error_code_)
384  {
385  case CAM_ERROR_BUSY:
386  ROS_ERROR("Error: CAM_ERROR_BUSY");
387  break;
388  case CAM_ERROR_PARAM:
389  ROS_ERROR("Error: CAM_ERROR_PARAM");
390  break;
391  case CAM_ERROR_MODE:
392  ROS_ERROR("Error: CAM_ERROR_MODE");
393  break;
394  default:
395  ROS_ERROR("Error: Unknown error response from camera.");
396  break;
397  }
398  return -1;
399 }
400 
401 /* These commands often have variable packet lengths, if there is an error,
402  * there is a smaller packet size. If we request the larger packet size first,
403  * then we will never get a response back. Because of this, we have to first
404  * request the smaller size, check if its a full packet, if it's not, request
405  * the rest of the packet. Also according to the source code for ARIA, we can
406  * not do more than 2 requests for a single packet, therefor, we can't just
407  * request 1 byte over and over again.
408  *
409  * So here, s1 is the size of the smaller packet.
410  * And s2 is the size of the larger packet.
411  */
412 int P2OSPtz::receiveRequestAnswer(unsigned char *data, int s1, int s2)
413 {
414  int num;
415  unsigned char reply[MAX_REQUEST_LENGTH];
416  int len = 0;
417  unsigned char byte;
418  int t;
419 
420  memset(reply, 0, MAX_REQUEST_LENGTH);
421  getPtzPacket(s1, s2);
422 
423  for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
424  {
425  // if we don't get any bytes, or if we've just exceeded the limit
426  // then return null
427  t = cb_.getFromBuf();
428  if ( t < 0 ) { // Buf Error!
429  ROS_ERROR("circbuf error!\n");
430  return -1;
431  }
432  else {
433  byte = (unsigned char)t;
434  }
435  if (byte == (unsigned char)RESPONSE)
436  {
437  reply[0] = byte;
438  len ++;
439  break;
440  }
441  }
442  if (len == 0)
443  {
444  ROS_ERROR("Received Request Answer has length 0");
445  return -1;
446  }
447  // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
448  for(num = 1; num <= MAX_REQUEST_LENGTH; num++)
449  {
450  t = cb_.getFromBuf();
451  if (t < 0)
452  {
453  // there are no more bytes, so check the last byte for the footer
454  if (reply[len - 1] != (unsigned char)FOOTER)
455  {
456  ROS_ERROR("Last Byte was not the footer!");
457  return -1;
458  }
459  else
460  break;
461  }
462  else
463  {
464  // add the byte to the array
465  reply[len] = (unsigned char)t;
466  len ++;
467  }
468  }
469  // Check the response length: pt: 14; zoom: 10
470  if (len != COMMAND_RESPONSE_BYTES && len != 8 && len != 10 && len != 14)
471  {
472  ROS_ERROR("Response Length was incorrect at %i.", len);
473  return -1;
474  }
475 
476  if (reply[0] != (unsigned char)RESPONSE ||
477  reply[len - 1] != (unsigned char)FOOTER)
478  {
479  ROS_ERROR("Header or Footer is wrong on received packet");
480  return -1;
481  }
482 
483  // so far so good. Set myError to the error byte
484  error_code_ = reply[3];
485 
487  {
488  memcpy(data, reply, len);
489  return len;
490  }
491  switch(error_code_)
492  {
493  case CAM_ERROR_BUSY:
494  ROS_ERROR("Error: CAM_ERROR_BUSY");
495  break;
496  case CAM_ERROR_PARAM:
497  ROS_ERROR("Error: CAM_ERROR_PARAM");
498  break;
499  case CAM_ERROR_MODE:
500  ROS_ERROR("Error: CAM_ERROR_MODE");
501  break;
502  default:
503  ROS_ERROR("Error: Unknown error response from camera.");
504  break;
505  }
506  return -1;
507 }
508 
509 void P2OSPtz::getPtzPacket(int s1, int s2)
510 {
511  int packetCount = 0;
512  unsigned char request[4];
513  P2OSPacket request_pkt;
514  bool secondSent = false;
515 
516  request[0] = GETAUX;
517  request[1] = ARGINT;
518  request[2] = s1;
519  request[3] = 0;
520 
521  // Reset our receiving buffer.
522  cb_.reset();
523 
524  //Request the request-size back
525  request_pkt.Build(request,4);
526  p2os_->SendReceive(&request_pkt,false);
527 
528  while ( !cb_.gotPacket() )
529  {
530  if ( packetCount++ > PACKET_TIMEOUT ) {
531  // Give Up We're not getting it.
532  ROS_ERROR("Waiting for packet timed out.");
533  return;
534  }
535  if ( cb_.size() == s1 && !secondSent)
536  {
537  if ( s2 > s1 )
538  {
539  // We got the first packet size, but we don't have a full packet.
540  int newsize = s2 - s1;
541  ROS_ERROR("Requesting Second Packet of size %i.", newsize);
542  request[2] = newsize;
543  request_pkt.Build(request,4);
544  secondSent = true;
545  p2os_->SendReceive(&request_pkt,false);
546  }
547  else
548  {
549  // We got the first packet but don't have a full packet, this is an error.
550  ROS_ERROR("Got reply from AUX1 But don't have a full packet.");
551  break;
552  }
553  }
554 
555  // Keep reading data until we get a response from the camera.
556  p2os_->SendReceive(NULL,false);
557  }
558 }
559 
560 //
561 // Device Commands
562 //
564 {
565  unsigned char command[MAX_COMMAND_LENGTH];
566 
567  command[0] = HEADER;
568  command[1] = DEVICEID;
569  command[2] = DEVICEID;
570  command[3] = DELIM;
571  command[4] = POWER;
572  if (on)
573  command[5] = DEVICEID + 1;
574  else
575  command[5] = DEVICEID;
576  command[6] = FOOTER;
577 
578  if (sendCommand(command, 7))
579  return -1;
580  if (bidirectional_com_)
581  {
583  }
584  else
585  {
586  usleep(SLEEP_TIME_USEC);
587  return 0;
588  }
589 }
590 
592 {
593  unsigned char command[MAX_COMMAND_LENGTH];
594 
595  command[0] = HEADER;
596  command[1] = DEVICEID;
597  command[2] = DEVICEID;
598  command[3] = DELIM;
599  command[4] = CONTROL;
600  command[5] = DEVICEID;
601  command[6] = FOOTER;
602 
603  if (sendCommand(command, 7))
604  return -1;
605  if (bidirectional_com_)
606  {
608  }
609  else
610  {
611  usleep(SLEEP_TIME_USEC);
612  return 0;
613  }
614 }
615 
617 {
618  unsigned char command[MAX_COMMAND_LENGTH];
619 
620  command[0] = HEADER;
621  command[1] = DEVICEID;
622  command[2] = DEVICEID;
623  command[3] = DELIM;
624  command[4] = INIT;
625  command[5] = DEVICEID;
626  command[6] = FOOTER;
627 
628  if (sendCommand(command, 7))
629  return -1;
630  if (bidirectional_com_)
631  {
633  }
634  else
635  {
636  usleep(SLEEP_TIME_USEC);
637  return 0;
638  }
639 }
640 
641 int P2OSPtz::getMaxZoom(int * maxzoom)
642 {
643  unsigned char command[MAX_COMMAND_LENGTH];
644  unsigned char reply[MAX_REQUEST_LENGTH];
645  int reply_len;
646  char byte;
647  unsigned char buf[4];
648  unsigned int u_zoom;
649  int i;
650 
651  command[0] = HEADER;
652  command[1] = DEVICEID;
653  command[2] = DEVICEID;
654  command[3] = DELIM;
655  command[4] = ZOOMREQ;
656  command[5] = DEVICEID;
657  command[6] = FOOTER;
658 
659  if (sendCommand(command, 7))
660  return -1;
661  // usleep(5000000);
662  if (bidirectional_com_)
663  {
664  reply_len = receiveRequestAnswer(reply,10,0);
665  }
666  else
667  {
668  return 0;
669  }
670 
671  if ( reply_len == COMMAND_RESPONSE_BYTES ){
672  return -1;
673  }
674 
675  // remove the ascii encoding, and put into 2 bytes
676  for (i = 0; i < 4; i++)
677  {
678  byte = reply[i + 5];
679  if (byte < 0x40)
680  byte = byte - 0x30;
681  else
682  byte = byte - 'A' + 10;
683  buf[i] = byte;
684  }
685 
686  // convert the 2 bytes into a number
687  u_zoom = 0;
688  for (i = 0; i < 4; i++)
689  u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i));
690  *maxzoom = u_zoom;
691 
692  return 0;
693 }
694 int P2OSPtz::getAbsZoom(int* zoom)
695 {
696  unsigned char command[MAX_COMMAND_LENGTH];
697  unsigned char reply[MAX_REQUEST_LENGTH];
698  int reply_len;
699  char byte;
700  unsigned char buf[4];
701  unsigned int u_zoom;
702  int i;
703 
704  command[0] = HEADER;
705  command[1] = DEVICEID;
706  command[2] = DEVICEID;
707  command[3] = DELIM;
708  command[4] = ZOOMREQ;
709  command[5] = DEVICEID;
710  command[6] = FOOTER;
711 
712  if (sendRequest(command, 6, reply))
713  return(-1);
714  if (bidirectional_com_)
715  {
716  reply_len = receiveRequestAnswer(reply,10,0);
717  }
718  else
719  {
720  return 0;
721  }
722 
723  if (reply_len == COMMAND_RESPONSE_BYTES)
724  return -1;
725 
726  // remove the ascii encoding, and put into 2 bytes
727  for (i = 0; i < 4; i++)
728  {
729  byte = reply[i + 5];
730  if (byte < 0x40)
731  byte = byte - 0x30;
732  else
733  byte = byte - 'A' + 10;
734  buf[i] = byte;
735  }
736 
737  // convert the 2 bytes into a number
738  u_zoom = 0;
739  for (i = 0; i < 4; i++)
740  u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i));
741  *zoom = u_zoom;
742  return(0);
743 }
744 
745 int P2OSPtz::sendAbsZoom(int zoom)
746 {
747  unsigned char command[MAX_COMMAND_LENGTH];
748  unsigned char buf[5];
749  int i;
750 
751  if(zoom < 0)
752  zoom = 0;
753 
754  else
755  if(zoom > max_zoom_){
756  zoom = max_zoom_;
757  }
758 
759  command[0] = HEADER;
760  command[1] = DEVICEID;
761  command[2] = DEVICEID;
762  command[3] = DELIM;
763  command[4] = ZOOM;
764 
765  sprintf((char *)buf, "%4X", zoom);
766 
767  for (i=0;i<3;i++)
768  if (buf[i] == ' ')
769  buf[i] = '0';
770 
771  // zoom position
772  command[5] = buf[0];
773  command[6] = buf[1];
774  command[7] = buf[2];
775  command[8] = buf[3];
776  command[9] = FOOTER;
777 
778  zoom_ = zoom;
779 
780  if (sendCommand(command, 10))
781  return -1;
782  if (bidirectional_com_)
783  {
785  }
786  else
787  {
788  usleep(SLEEP_TIME_USEC);
789  return 0;
790  }
791  //return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
792 }
793 
795 {
796  unsigned char command[MAX_COMMAND_LENGTH];
797  unsigned char buf[5]; //4 values and string terminator
798  int maxtilt, mintilt;
799 
800  command[0] = HEADER;
801  command[1] = DEVICEID;
802  command[2] = DEVICEID;
803  command[3] = DELIM;
804  command[4] = SETRANGE;
805  command[5] = DEVICEID+1;
806 
807  mintilt = (int)(floor(MIN_TILT/.1125) + 0x8000);
808  sprintf((char*)buf, "%X", mintilt);
809  command[6] = buf[0];
810  command[7] = buf[1];
811  command[8] = buf[2];
812  command[9] = buf[3];
813  maxtilt = (int)(floor(MAX_TILT/.1125) + 0x8000);
814  sprintf((char*)buf, "%X", maxtilt);
815 
816  command[10] = buf[0];
817  command[11] = buf[1];
818  command[12] = buf[2];
819  command[13] = buf[3];
820  command[14] = FOOTER;
821 
822  if(sendCommand(command, 15))
823  return -1;
824  if (bidirectional_com_)
825  {
827  }
828  else
829  {
830  usleep(SLEEP_TIME_USEC);
831  return 0;
832  }
833 
834  // return(receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
835 
836 }
837 
838 int P2OSPtz::getAbsPanTilt(int* pan, int* tilt)
839 {
840  unsigned char command[MAX_COMMAND_LENGTH];
841  unsigned char reply[MAX_REQUEST_LENGTH];
842  int reply_len;
843  unsigned char buf[4];
844  char byte;
845  unsigned int u_val;
846  int val;
847  int i;
848 
849  command[0] = HEADER;
850  command[1] = DEVICEID;
851  command[2] = DEVICEID;
852  command[3] = DELIM;
853  command[4] = PANTILTREQ;
854  command[5] = FOOTER;
855 
856  if (sendRequest(command, 6, reply))
857  return(-1);
858  if (bidirectional_com_)
859  {
860  reply_len = receiveRequestAnswer(reply,14,0);
861  }
862  else
863  {
864  return 0;
865  }
866 
867  if ( reply_len != 14 ) {
868  ROS_ERROR("Reply Len = %i; should equal 14", reply_len);
869  return -1;
870  }
871 
872  // remove the ascii encoding, and put into 4-byte array
873  for (i = 0; i < 4; i++)
874  {
875  byte = reply[i+5];
876  if (byte < 0x40)
877  byte = byte - 0x30;
878  else
879  byte = byte - 'A' + 10;
880  buf[i] = byte;
881  }
882 
883  // convert the 4-bytes into a number
884  u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
885 
886  // convert the number to a value that's meaningful, based on camera specs
887  val = (int)(((int)u_val - (int)0x8000) * 0.1125);
888 
889  // now set myPan to the response received for where the camera thinks it is
890  *pan = val;
891 
892  // repeat the steps for the tilt value
893  for (i = 0; i < 4; i++)
894  {
895  byte = reply[i+9];
896  if (byte < 0x40)
897  byte = byte - 0x30;
898  else
899  byte = byte - 'A' + 10;
900  buf[i] = byte;
901  }
902  u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
903  val =(int)(((int)u_val - (int)0x8000) * 0.1125);
904  *tilt = val;
905 
906  return(0);
907 }
908 
909 int P2OSPtz::sendAbsPanTilt(int pan, int tilt)
910 {
911  unsigned char command[MAX_COMMAND_LENGTH];
912  int convpan, convtilt;
913  unsigned char buf[5];
914  int ppan, ttilt;
915 
916  ppan = pan; ttilt = tilt;
917  if(pan < MIN_PAN)
918  {
919  ppan = (int)MIN_PAN;
920  }
921  else if(pan > MAX_PAN)
922  {
923  ppan = (int)MAX_PAN;
924  }
925 
926  if (tilt > MAX_TILT)
927  {
928  ttilt = (int)MAX_TILT;
929  }
930  else if(tilt < MIN_TILT)
931  {
932  ttilt = (int)MIN_TILT;
933  }
934  //puts("Camera pan angle thresholded");
935 
936  //puts("Camera tilt angle thresholded");
937 
938  convpan = (int)floor(ppan/.1125) + 0x8000;
939  convtilt = (int)floor(ttilt/.1125) + 0x8000;
940  // fprintf(stdout, "ppan: %d ttilt: %d conpan: %d contilt: %d\n",
941  // ppan,ttilt,convpan,convtilt);
942  command[0] = HEADER;
943  command[1] = DEVICEID;
944  command[2] = DEVICEID;
945  command[3] = DELIM;
946  command[4] = PANTILT;
947  // pan position
948 
949  snprintf((char *)buf,sizeof(buf), "%X", convpan);
950 
951  command[5] = buf[0];
952  command[6] = buf[1];
953  command[7] = buf[2];
954  command[8] = buf[3];
955  // tilt position
956  snprintf((char *)buf,sizeof(buf), "%X", convtilt);
957  command[9] = buf[0];
958  command[10] = buf[1];
959  command[11] = buf[2];
960  command[12] = buf[3];
961  command[13] = (unsigned char) FOOTER;
962  if(sendCommand(command, 14))
963  return -1;
964 
965  tilt_ = ttilt;
966  pan_ = ppan;
967 
968  if (bidirectional_com_)
969  {
971  }
972  else
973  {
974  usleep(SLEEP_TIME_USEC);
975  return 0;
976  }
977 
978  //return(receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
979 }
980 
981 //
982 // Circular Buffer To deal with getting data back from AUX
983 //
984 circbuf::circbuf(int size) :
985  start(0), end(0), mysize(size), gotPack(false)
986 {
987  this->buf = new unsigned char[size];
988 }
989 
991  int i = start;
992  printf("circbuf: ");
993  while ( i != end ){
994  printf("0x%x ", buf[i]);
995  i = (i+1)%mysize;
996  }
997  printf("\n");
998 }
999 
1000 
1001 void circbuf::putOnBuf(unsigned char c)
1002 {
1003  buf[end] = c;
1004  end = (end+1)%mysize;
1005  if ( end == start )
1006  {
1007  // We're overwriting old data.
1008  start = (start + 1)%mysize;
1009  }
1010 
1011  // Check to see if we have the whole packet now. (ends with FOOTER)
1012  if ( c == P2OSPtz::FOOTER )
1013  {
1014  gotPack = true;
1015  }
1016 }
1017 
1019 {
1020  return !(this->start == this->end);
1021 }
1022 
1024 {
1025  if ( start != end ){
1026  unsigned char ret = buf[start];
1027  start = (start+1)%mysize;
1028  return (int)ret;
1029  }
1030  else
1031  {
1032  return -1;
1033  }
1034 }
1035 
1037 {
1038  if ( end > start )
1039  {
1040  return end-start;
1041  }
1042  else if ( start > end )
1043  {
1044  return mysize - start - end - 1;
1045  }
1046  else
1047  {
1048  return 0;
1049  }
1050 }
1051 
1053 {
1054  return gotPack;
1055 }
1056 
1058 {
1059  memset(buf, 0, mysize);
1060  gotPack = false;
1061  start = end = 0;
1062 }
int getFromBuf()
Definition: p2os_ptz.cpp:1023
int end
Definition: p2os_ptz.h:51
Initializes the camera.
Definition: p2os_ptz.h:65
int max_zoom_
Definition: p2os_ptz.h:170
int size()
Definition: p2os_ptz.cpp:1036
Pan/tilt command.
Definition: p2os_ptz.h:68
Illegal parameters to function call.
Definition: p2os_ptz.h:120
static const int MAX_COMMAND_LENGTH
Definition: p2os_ptz.h:178
bool gotPack
Definition: p2os_ptz.h:53
int SendReceive(P2OSPacket *pkt, bool publish_data=true)
Definition: p2os.cc:724
Requests max zoom position.
Definition: p2os_ptz.h:81
int setControlMode()
Definition: p2os_ptz.cpp:591
P2OSPtz(P2OSNode *p2os, bool bidirectional_com=false)
Definition: p2os_ptz.cpp:41
void shutdown()
Definition: p2os_ptz.cpp:151
Request pan/tilt position.
Definition: p2os_ptz.h:70
int setDefaultTiltRange()
Definition: p2os_ptz.cpp:794
P2OSNode * p2os_
Definition: p2os_ptz.h:166
Pan/tilt min/max range assignment.
Definition: p2os_ptz.h:69
Packet Footer.
Definition: p2os_ptz.h:84
bool gotPacket()
Definition: p2os_ptz.cpp:1052
int error_code_
Definition: p2os_ptz.h:173
int sendCommand(unsigned char *str, int len)
Definition: p2os_ptz.cpp:226
Puts camera in Control mode.
Definition: p2os_ptz.h:74
int getAbsPanTilt(int *pan, int *tilt)
Definition: p2os_ptz.cpp:838
static const int PACKET_TIMEOUT
Definition: p2os_ptz.h:181
int receiveCommandAnswer(int asize)
Definition: p2os_ptz.cpp:299
Not in host control mode.
Definition: p2os_ptz.h:121
#define ARGINT
Definition: robot_params.h:202
bool bidirectional_com_
Definition: p2os_ptz.h:174
int mysize
Definition: p2os_ptz.h:52
void putOnBuf(unsigned char c)
Definition: p2os_ptz.cpp:1001
circbuf cb_
Definition: p2os_ptz.h:168
Packet header for response.
Definition: p2os_ptz.h:85
int sendAbsZoom(int zoom)
Definition: p2os_ptz.cpp:745
int Build(unsigned char *data, unsigned char datasize)
Definition: packet.cc:134
int setPower(Power on)
Definition: p2os_ptz.cpp:563
static const int TILT_THRESH
Definition: p2os_ptz.h:184
int zoom_
Definition: p2os_ptz.h:171
Default device ID.
Definition: p2os_ptz.h:61
void getPtzPacket(int s1, int s2=0)
Definition: p2os_ptz.cpp:509
static const int SLEEP_TIME_USEC
Definition: p2os_ptz.h:182
void callback(const p2os_msgs::PTZStateConstPtr &msg)
Definition: p2os_ptz.cpp:162
static const int COMMAND_RESPONSE_BYTES
Definition: p2os_ptz.h:180
int tilt_
Definition: p2os_ptz.h:171
Delimeter character.
Definition: p2os_ptz.h:60
int sendInit()
Definition: p2os_ptz.cpp:616
int sendAbsPanTilt(int pan, int tilt)
Definition: p2os_ptz.cpp:909
Packet Header.
Definition: p2os_ptz.h:86
static const int PAN_THRESH
Definition: p2os_ptz.h:183
Turns on/off power.
Definition: p2os_ptz.h:75
int receiveRequestAnswer(unsigned char *data, int s1, int s2)
Definition: p2os_ptz.cpp:412
int start
Definition: p2os_ptz.h:50
bool is_on_
Definition: p2os_ptz.h:172
Zooms camera lens.
Definition: p2os_ptz.h:80
static const int MAX_REQUEST_LENGTH
Definition: p2os_ptz.h:179
unsigned char * buf
Definition: p2os_ptz.h:49
bool haveData()
Definition: p2os_ptz.cpp:1018
static const int ZOOM_THRESH
Definition: p2os_ptz.h:185
int getAbsZoom(int *zoom)
Definition: p2os_ptz.cpp:694
int setup()
Definition: p2os_ptz.cpp:56
int sendRequest(unsigned char *str, int len, unsigned char *reply)
Definition: p2os_ptz.cpp:262
void printBuf()
Definition: p2os_ptz.cpp:990
#define ARGSTR
Definition: robot_params.h:204
Definition: p2os.h:86
Camera busy, will not execute the command.
Definition: p2os_ptz.h:119
void reset()
Definition: p2os_ptz.cpp:1057
int pan_
Definition: p2os_ptz.h:171
int getMaxZoom(int *max_zoom)
Definition: p2os_ptz.cpp:641
p2os_msgs::PTZState current_state_
Definition: p2os_ptz.h:175
circbuf(int size=512)
Definition: p2os_ptz.cpp:984


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Jun 25 2014 09:37:15