p2os_ptz.cpp
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2000
00004  *     Tucker Hermans
00005  *
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 #include <p2os_driver/p2os_ptz.h>
00024 #include <p2os_driver/p2os.h>
00025 
00026 //
00027 // Constants
00028 //
00029 const int P2OSPtz::MAX_COMMAND_LENGTH = 19;
00030 const int P2OSPtz::MAX_REQUEST_LENGTH = 17;
00031 const int P2OSPtz::COMMAND_RESPONSE_BYTES = 6;
00032 const int P2OSPtz::PACKET_TIMEOUT = 300;
00033 const int P2OSPtz::SLEEP_TIME_USEC = 300000;
00034 const int P2OSPtz::PAN_THRESH = 1;
00035 const int P2OSPtz::TILT_THRESH = 1;
00036 const int P2OSPtz::ZOOM_THRESH = 1;
00037 
00038 //
00039 // Constructors
00040 //
00041 P2OSPtz::P2OSPtz(P2OSNode * p2os, bool bidirectional_com) :
00042     p2os_(p2os), max_zoom_(MAX_ZOOM_OPTIC), pan_(0), tilt_(0), zoom_(0),
00043     is_on_(false), error_code_(CAM_ERROR_NONE),
00044     bidirectional_com_(bidirectional_com)
00045 {
00046   current_state_.pan = 0;
00047   current_state_.zoom = 0;
00048   current_state_.tilt = 0;
00049   current_state_.relative = false;
00050 }
00051 
00052 
00053 //
00054 // Core Functions
00055 //
00056 int P2OSPtz::setup()
00057 {
00058   int err = 0;
00059   int num_inits = 7;
00060   is_on_ = true;
00061   for (int i = 1; i < num_inits; i++) {
00062     switch(i)
00063     {
00064       // case 0:
00065       //   do
00066       //   {
00067       //     ROS_DEBUG("Waiting for camera to power off.");
00068       //     err = setPower(POWER_OFF);
00069       //   } while (error_code_ == CAM_ERROR_BUSY);
00070       //   break;
00071       case 1:
00072         do
00073         {
00074           ROS_DEBUG("Waiting for camera to power on.");
00075           err = setPower(POWER_ON);
00076         } while (error_code_ == CAM_ERROR_BUSY);
00077         break;
00078       case 2:
00079         do
00080         {
00081           ROS_DEBUG("Waiting for camera mode to set");
00082           err = setControlMode();
00083         } while (error_code_ == CAM_ERROR_BUSY);
00084         break;
00085       case 3:
00086         do
00087         {
00088           ROS_DEBUG("Waiting for camera to initialize");
00089           err = sendInit();
00090         } while (error_code_ == CAM_ERROR_BUSY);
00091         break;
00092       case 4:
00093         do
00094         {
00095           for(int i = 0; i < 3; i++)
00096           {
00097             ROS_DEBUG("Waiting for camera to set default tilt");
00098             err = setDefaultTiltRange();
00099           }
00100         } while (error_code_ == CAM_ERROR_BUSY);
00101         break;
00102       case 5:
00103         do
00104         {
00105           ROS_DEBUG("Waiting for camera to set initial pan and tilt");
00106           err = sendAbsPanTilt(0, 0);
00107         } while (error_code_ == CAM_ERROR_BUSY);
00108         break;
00109       case 6:
00110         do
00111         {
00112           ROS_DEBUG("Waiting for camera to set initial zoom");
00113           err = sendAbsZoom(0);
00114         } while (error_code_ == CAM_ERROR_BUSY);
00115         break;
00116       default:
00117         err = -7;
00118         break;
00119     }
00120 
00121     // Check for erros after each attempt
00122     if (err)
00123     {
00124       ROS_ERROR("Error initiliazing PTZ at stage %i", i);
00125       switch(error_code_)
00126       {
00127         case CAM_ERROR_BUSY:
00128           ROS_ERROR("Error: CAM_ERROR_BUSY");
00129           break;
00130         case CAM_ERROR_PARAM:
00131           ROS_ERROR("Error: CAM_ERROR_PARAM");
00132           break;
00133         case CAM_ERROR_MODE:
00134           ROS_ERROR("Error: CAM_ERROR_MODE");
00135           break;
00136         default:
00137           ROS_ERROR("Error: Unknown error response from camera.");
00138           break;
00139       }
00140       return(-1);
00141     }
00142     else
00143     {
00144       ROS_DEBUG("Passed stage %i of PTZ initialization.", i);
00145     }
00146   }
00147   ROS_DEBUG("Finished initialization of the PTZ.");
00148   return 0;
00149 }
00150 
00151 void P2OSPtz::shutdown()
00152 {
00153   sendAbsPanTilt(0,0);
00154   usleep(SLEEP_TIME_USEC);
00155   sendAbsZoom(0);
00156   usleep(SLEEP_TIME_USEC);
00157   setPower(POWER_OFF);
00158   usleep(SLEEP_TIME_USEC);
00159   ROS_INFO("PTZ camera has been shutdown");
00160 }
00161 
00162 void P2OSPtz::callback(const p2os_msgs::PTZStateConstPtr &cmd)
00163 {
00164   p2os_msgs::PTZState to_send;
00165   bool change_pan_tilt = false;
00166   bool change_zoom = false;
00167   to_send.pan = pan_;
00168   to_send.tilt = tilt_;
00169   to_send.zoom = zoom_;
00170 
00171   // Check if the command is relative to the current position
00172   if (cmd->relative)
00173   {
00174     if ( abs(cmd->pan) > PAN_THRESH)
00175     {
00176       to_send.pan = cmd->pan + pan_;
00177       change_pan_tilt = true;
00178     }
00179     if ( abs(cmd->tilt) > TILT_THRESH)
00180     {
00181       to_send.tilt = cmd->tilt + tilt_;
00182       change_pan_tilt = true;
00183     }
00184     if ( abs(cmd->zoom) > ZOOM_THRESH)
00185     {
00186       to_send.zoom = cmd->zoom + zoom_;
00187       change_zoom = true;
00188     }
00189   }
00190   else
00191   {
00192     if ( abs(cmd->pan - pan_) > PAN_THRESH)
00193     {
00194       to_send.pan = cmd->pan;
00195       change_pan_tilt = true;
00196     }
00197     if ( abs(cmd->tilt - tilt_) > TILT_THRESH)
00198     {
00199       to_send.tilt = cmd->tilt;
00200       change_pan_tilt = true;
00201     }
00202     if ( abs(cmd->zoom - zoom_) > ZOOM_THRESH)
00203     {
00204       to_send.zoom = cmd->zoom;
00205       change_zoom = true;
00206     }
00207   }
00208 
00209   if (change_pan_tilt)
00210   {
00211     sendAbsPanTilt(to_send.pan, to_send.tilt);
00212   }
00213   if (change_zoom)
00214   {
00215     sendAbsZoom(to_send.zoom);
00216   }
00217 
00218   current_state_.pan = pan_;
00219   current_state_.zoom = zoom_;
00220   current_state_.tilt = tilt_;
00221 }
00222 
00223 //
00224 // Communication Functions
00225 //
00226 int P2OSPtz::sendCommand(unsigned char *str, int len)
00227 {
00228   P2OSPacket ptz_packet;
00229   P2OSPacket request_pkt;
00230   unsigned char request[4];
00231 
00232   // Zero out the Receive Buffer
00233   request[0] = GETAUX;
00234   request[1] = ARGINT;
00235   request[2] = 0;
00236   request[3] = 0;
00237   request_pkt.Build(request,4);
00238   p2os_->SendReceive(&request_pkt,false);
00239 
00240   if(len > MAX_COMMAND_LENGTH)
00241   {
00242     ROS_ERROR("Command message is too large to send");
00243     return(-1);
00244   }
00245 
00246   // Since I'm hardcoding this to AUX1, basically we gotta stick the AUX1DATA
00247   // header on this and then give it to the p2os send command.
00248   unsigned char mybuf[MAX_COMMAND_LENGTH+3];
00249   mybuf[0] = TTY2;
00250   mybuf[1] = ARGSTR;
00251   mybuf[2] = len;
00252   // Copy the command
00253   memcpy(&mybuf[3], str, len);
00254   ptz_packet.Build(mybuf, len+3);
00255 
00256   // Send the packet
00257   p2os_->SendReceive(&ptz_packet, false);
00258 
00259   return(0);
00260 }
00261 
00262 int P2OSPtz::sendRequest(unsigned char *str, int len, unsigned char *reply)
00263 {
00264   P2OSPacket ptz_packet;
00265   P2OSPacket request_pkt;
00266   unsigned char request[4];
00267 
00268   // Zero out the Receive Buffer
00269   request[0] = GETAUX;
00270   request[1] = ARGINT;
00271   request[2] = 0;
00272   request[3] = 0;
00273   request_pkt.Build(request,4);
00274   p2os_->SendReceive(&request_pkt,false);
00275 
00276   if (len > MAX_REQUEST_LENGTH)
00277   {
00278     ROS_ERROR("Request message is too large to send.");
00279     return -1;
00280   }
00281 
00282   // Since I'm hardcoding this to AUX1, basically we gotta stick the AUX1DATA
00283   // header on this and then give it to the p2os send command.
00284   unsigned char mybuf[MAX_REQUEST_LENGTH];
00285   mybuf[0] = TTY2;
00286   mybuf[1] = ARGSTR;
00287   mybuf[2] = len;
00288   // Copy the command
00289   memcpy(&mybuf[3], str, len);
00290   ptz_packet.Build(mybuf, len+3);
00291 
00292   // Send the packet
00293   p2os_->SendReceive(&ptz_packet, false);
00294 
00295 
00296   return 0;
00297 }
00298 
00299 int P2OSPtz::receiveCommandAnswer(int asize)
00300 {
00301   int num;
00302   unsigned char reply[MAX_REQUEST_LENGTH];
00303   int len = 0;
00304   unsigned char byte;
00305   int t;
00306   memset(reply, 0, COMMAND_RESPONSE_BYTES);
00307 
00308   getPtzPacket(asize);
00309 
00310   for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
00311   {
00312     // if we don't get any bytes, or if we've just exceeded the limit
00313     // then return null
00314     t = cb_.getFromBuf();
00315     if ( t < 0 )
00316     {
00317       // Buf Error!
00318       ROS_ERROR("circbuf error!");
00319       return -1;
00320     }
00321     else
00322     {
00323       byte = (unsigned char)t;
00324     }
00325     if (byte == (unsigned char)RESPONSE)
00326     {
00327       reply[0] = byte;
00328       len ++;
00329       break;
00330     }
00331   }
00332 
00333   if (len == 0)
00334   {
00335     ROS_ERROR("Length is 0 on received packet.");
00336     return -1;
00337   }
00338 
00339   // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
00340   for(num = 1; num <= MAX_REQUEST_LENGTH; num++)
00341   {
00342     t = cb_.getFromBuf();
00343     if (t < 0)
00344     {
00345       // there are no more bytes, so check the last byte for the footer
00346       if (reply[len - 1] !=  (unsigned char)FOOTER)
00347       {
00348         ROS_ERROR("canonvcc4::receiveCommandAnswer: Discarding bad packet.");
00349         return -1;
00350       }
00351       else
00352         break;
00353     }
00354     else
00355     {
00356       // add the byte to the array
00357       reply[len] = (unsigned char)t;
00358       len ++;
00359     }
00360   }
00361 
00362   // Check the response
00363   if (len != COMMAND_RESPONSE_BYTES)
00364   {
00365     ROS_ERROR("Answer does not equal command response bytes");
00366     return -1;
00367   }
00368 
00369   // check the header and footer
00370   if (reply[0] != (unsigned char)RESPONSE || reply[5] != (unsigned char)FOOTER)
00371   {
00372     ROS_ERROR("Header or Footer is wrong on received packet");
00373     return -1;
00374   }
00375 
00376   // so far so good.  Set myError to the error byte
00377   error_code_ = reply[3];
00378   if (error_code_ == CAM_ERROR_NONE)
00379   {
00380     return 0;
00381   }
00382 
00383   switch(error_code_)
00384   {
00385     case CAM_ERROR_BUSY:
00386       ROS_ERROR("Error: CAM_ERROR_BUSY");
00387       break;
00388     case CAM_ERROR_PARAM:
00389       ROS_ERROR("Error: CAM_ERROR_PARAM");
00390       break;
00391     case CAM_ERROR_MODE:
00392       ROS_ERROR("Error: CAM_ERROR_MODE");
00393       break;
00394     default:
00395       ROS_ERROR("Error: Unknown error response from camera.");
00396       break;
00397   }
00398   return -1;
00399 }
00400 
00401 /* These commands often have variable packet lengths, if there is an error,
00402  * there is a smaller packet size. If we request the larger packet size first,
00403  * then we will never get a response back. Because of this, we have to first
00404  * request the smaller size, check if its a full packet, if it's not, request
00405  * the rest of the packet. Also according to the source code for ARIA, we can
00406  * not do more than 2 requests for a single packet, therefor, we can't just
00407  * request 1 byte over and over again.
00408  *
00409  * So here, s1 is the size of the smaller packet.
00410  * And s2 is the size of the larger packet.
00411  */
00412 int P2OSPtz::receiveRequestAnswer(unsigned char *data, int s1, int s2)
00413 {
00414   int num;
00415   unsigned char reply[MAX_REQUEST_LENGTH];
00416   int len = 0;
00417   unsigned char byte;
00418   int t;
00419 
00420   memset(reply, 0, MAX_REQUEST_LENGTH);
00421   getPtzPacket(s1, s2);
00422 
00423   for (num = 0; num <= COMMAND_RESPONSE_BYTES + 1; num++)
00424   {
00425     // if we don't get any bytes, or if we've just exceeded the limit
00426     // then return null
00427     t = cb_.getFromBuf();
00428     if ( t < 0 ) { // Buf Error!
00429       ROS_ERROR("circbuf error!\n");
00430       return -1;
00431     }
00432     else {
00433       byte = (unsigned char)t;
00434     }
00435     if (byte == (unsigned char)RESPONSE)
00436     {
00437       reply[0] = byte;
00438       len ++;
00439       break;
00440     }
00441   }
00442   if (len == 0)
00443   {
00444     ROS_ERROR("Received Request Answer has length 0");
00445     return -1;
00446   }
00447   // we got the header character so keep reading bytes for MAX_RESPONSE_BYTES more
00448   for(num = 1; num <= MAX_REQUEST_LENGTH; num++)
00449   {
00450     t = cb_.getFromBuf();
00451     if (t < 0)
00452     {
00453       // there are no more bytes, so check the last byte for the footer
00454       if (reply[len - 1] !=  (unsigned char)FOOTER)
00455       {
00456         ROS_ERROR("Last Byte was not the footer!");
00457         return -1;
00458       }
00459       else
00460         break;
00461     }
00462     else
00463     {
00464       // add the byte to the array
00465       reply[len] = (unsigned char)t;
00466       len ++;
00467     }
00468   }
00469   // Check the response length: pt: 14; zoom: 10
00470   if (len != COMMAND_RESPONSE_BYTES && len != 8 && len != 10 && len != 14)
00471   {
00472     ROS_ERROR("Response Length was incorrect at %i.", len);
00473     return -1;
00474   }
00475 
00476   if (reply[0] !=  (unsigned char)RESPONSE ||
00477       reply[len - 1] != (unsigned char)FOOTER)
00478   {
00479     ROS_ERROR("Header or Footer is wrong on received packet");
00480     return -1;
00481   }
00482 
00483   // so far so good.  Set myError to the error byte
00484   error_code_ = reply[3];
00485 
00486   if (error_code_ == CAM_ERROR_NONE)
00487   {
00488     memcpy(data, reply, len);
00489     return len;
00490   }
00491   switch(error_code_)
00492   {
00493     case CAM_ERROR_BUSY:
00494       ROS_ERROR("Error: CAM_ERROR_BUSY");
00495       break;
00496     case CAM_ERROR_PARAM:
00497       ROS_ERROR("Error: CAM_ERROR_PARAM");
00498       break;
00499     case CAM_ERROR_MODE:
00500       ROS_ERROR("Error: CAM_ERROR_MODE");
00501       break;
00502     default:
00503       ROS_ERROR("Error: Unknown error response from camera.");
00504       break;
00505   }
00506   return -1;
00507 }
00508 
00509 void P2OSPtz::getPtzPacket(int s1, int s2)
00510 {
00511   int packetCount = 0;
00512   unsigned char request[4];
00513   P2OSPacket request_pkt;
00514   bool secondSent = false;
00515 
00516   request[0] = GETAUX;
00517   request[1] = ARGINT;
00518   request[2] = s1;
00519   request[3] = 0;
00520 
00521   // Reset our receiving buffer.
00522   cb_.reset();
00523 
00524   //Request the request-size back
00525   request_pkt.Build(request,4);
00526   p2os_->SendReceive(&request_pkt,false);
00527 
00528   while ( !cb_.gotPacket() )
00529   {
00530     if ( packetCount++ > PACKET_TIMEOUT ) {
00531       // Give Up We're not getting it.
00532       ROS_ERROR("Waiting for packet timed out.");
00533       return;
00534     }
00535     if ( cb_.size() == s1 && !secondSent)
00536     {
00537       if ( s2 > s1 )
00538       {
00539         // We got the first packet size, but we don't have a full packet.
00540         int newsize = s2 - s1;
00541         ROS_ERROR("Requesting Second Packet of size %i.", newsize);
00542         request[2] = newsize;
00543         request_pkt.Build(request,4);
00544         secondSent = true;
00545         p2os_->SendReceive(&request_pkt,false);
00546       }
00547       else
00548       {
00549         // We got the first packet but don't have a full packet, this is an error.
00550         ROS_ERROR("Got reply from AUX1 But don't have a full packet.");
00551         break;
00552       }
00553     }
00554 
00555     // Keep reading data until we get a response from the camera.
00556     p2os_->SendReceive(NULL,false);
00557   }
00558 }
00559 
00560 //
00561 // Device Commands
00562 //
00563 int P2OSPtz::setPower(Power on)
00564 {
00565   unsigned char command[MAX_COMMAND_LENGTH];
00566 
00567   command[0] = HEADER;
00568   command[1] = DEVICEID;
00569   command[2] = DEVICEID;
00570   command[3] = DELIM;
00571   command[4] = POWER;
00572   if (on)
00573     command[5] = DEVICEID + 1;
00574   else
00575     command[5] = DEVICEID;
00576   command[6] = FOOTER;
00577 
00578   if (sendCommand(command, 7))
00579     return -1;
00580   if (bidirectional_com_)
00581   {
00582     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00583   }
00584   else
00585   {
00586     usleep(SLEEP_TIME_USEC);
00587     return 0;
00588   }
00589 }
00590 
00591 int P2OSPtz::setControlMode()
00592 {
00593   unsigned char command[MAX_COMMAND_LENGTH];
00594 
00595   command[0] = HEADER;
00596   command[1] = DEVICEID;
00597   command[2] = DEVICEID;
00598   command[3] = DELIM;
00599   command[4] = CONTROL;
00600   command[5] = DEVICEID;
00601   command[6] = FOOTER;
00602 
00603   if (sendCommand(command, 7))
00604     return -1;
00605   if (bidirectional_com_)
00606   {
00607     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00608   }
00609   else
00610   {
00611     usleep(SLEEP_TIME_USEC);
00612     return 0;
00613   }
00614 }
00615 
00616 int P2OSPtz::sendInit()
00617 {
00618   unsigned char command[MAX_COMMAND_LENGTH];
00619 
00620   command[0] = HEADER;
00621   command[1] = DEVICEID;
00622   command[2] = DEVICEID;
00623   command[3] = DELIM;
00624   command[4] = INIT;
00625   command[5] = DEVICEID;
00626   command[6] = FOOTER;
00627 
00628   if (sendCommand(command, 7))
00629     return -1;
00630   if (bidirectional_com_)
00631   {
00632     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00633   }
00634   else
00635   {
00636     usleep(SLEEP_TIME_USEC);
00637     return 0;
00638   }
00639 }
00640 
00641 int P2OSPtz::getMaxZoom(int * maxzoom)
00642 {
00643   unsigned char command[MAX_COMMAND_LENGTH];
00644   unsigned char reply[MAX_REQUEST_LENGTH];
00645   int reply_len;
00646   char byte;
00647   unsigned char buf[4];
00648   unsigned int u_zoom;
00649   int i;
00650 
00651   command[0] = HEADER;
00652   command[1] = DEVICEID;
00653   command[2] = DEVICEID;
00654   command[3] = DELIM;
00655   command[4] = ZOOMREQ;
00656   command[5] = DEVICEID;
00657   command[6] = FOOTER;
00658 
00659   if (sendCommand(command, 7))
00660     return -1;
00661   //  usleep(5000000);
00662   if (bidirectional_com_)
00663   {
00664     reply_len = receiveRequestAnswer(reply,10,0);
00665   }
00666   else
00667   {
00668     return 0;
00669   }
00670 
00671   if ( reply_len == COMMAND_RESPONSE_BYTES ){
00672     return -1;
00673   }
00674 
00675   // remove the ascii encoding, and put into 2 bytes
00676   for (i = 0; i < 4; i++)
00677   {
00678     byte = reply[i + 5];
00679     if (byte < 0x40)
00680       byte = byte - 0x30;
00681     else
00682       byte = byte - 'A' + 10;
00683     buf[i] = byte;
00684   }
00685 
00686   // convert the 2 bytes into a number
00687   u_zoom = 0;
00688   for (i = 0; i < 4; i++)
00689     u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i));
00690   *maxzoom = u_zoom;
00691 
00692   return 0;
00693 }
00694 int P2OSPtz::getAbsZoom(int* zoom)
00695 {
00696   unsigned char command[MAX_COMMAND_LENGTH];
00697   unsigned char reply[MAX_REQUEST_LENGTH];
00698   int reply_len;
00699   char byte;
00700   unsigned char buf[4];
00701   unsigned int u_zoom;
00702   int i;
00703 
00704   command[0] = HEADER;
00705   command[1] = DEVICEID;
00706   command[2] = DEVICEID;
00707   command[3] = DELIM;
00708   command[4] = ZOOMREQ;
00709   command[5] = DEVICEID;
00710   command[6] = FOOTER;
00711 
00712   if (sendRequest(command, 6, reply))
00713     return(-1);
00714   if (bidirectional_com_)
00715   {
00716     reply_len = receiveRequestAnswer(reply,10,0);
00717   }
00718   else
00719   {
00720     return 0;
00721   }
00722 
00723   if (reply_len == COMMAND_RESPONSE_BYTES)
00724     return -1;
00725 
00726   // remove the ascii encoding, and put into 2 bytes
00727   for (i = 0; i < 4; i++)
00728   {
00729     byte = reply[i + 5];
00730     if (byte < 0x40)
00731       byte = byte - 0x30;
00732     else
00733       byte = byte - 'A' + 10;
00734     buf[i] = byte;
00735   }
00736 
00737   // convert the 2 bytes into a number
00738   u_zoom = 0;
00739   for (i = 0; i < 4; i++)
00740     u_zoom += buf[i] * (unsigned int) pow(16.0, (double)(3 - i));
00741   *zoom = u_zoom;
00742   return(0);
00743 }
00744 
00745 int P2OSPtz::sendAbsZoom(int zoom)
00746 {
00747   unsigned char command[MAX_COMMAND_LENGTH];
00748   unsigned char buf[5];
00749   int i;
00750 
00751   if(zoom < 0)
00752     zoom = 0;
00753 
00754   else
00755     if(zoom > max_zoom_){
00756       zoom = max_zoom_;
00757     }
00758 
00759   command[0] = HEADER;
00760   command[1] = DEVICEID;
00761   command[2] = DEVICEID;
00762   command[3] = DELIM;
00763   command[4] = ZOOM;
00764 
00765   sprintf((char *)buf, "%4X", zoom);
00766 
00767   for (i=0;i<3;i++)
00768     if (buf[i] == ' ')
00769       buf[i] = '0';
00770 
00771   // zoom position
00772   command[5] = buf[0];
00773   command[6] = buf[1];
00774   command[7] = buf[2];
00775   command[8] = buf[3];
00776   command[9] = FOOTER;
00777 
00778   zoom_ = zoom;
00779 
00780   if (sendCommand(command, 10))
00781     return -1;
00782   if (bidirectional_com_)
00783   {
00784     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00785   }
00786   else
00787   {
00788     usleep(SLEEP_TIME_USEC);
00789     return 0;
00790   }
00791   //return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00792 }
00793 
00794 int P2OSPtz::setDefaultTiltRange()
00795 {
00796   unsigned char command[MAX_COMMAND_LENGTH];
00797   unsigned char buf[5]; //4 values and string terminator
00798   int maxtilt, mintilt;
00799 
00800   command[0] = HEADER;
00801   command[1] = DEVICEID;
00802   command[2] = DEVICEID;
00803   command[3] = DELIM;
00804   command[4] = SETRANGE;
00805   command[5] = DEVICEID+1;
00806 
00807   mintilt = (int)(floor(MIN_TILT/.1125) + 0x8000);
00808   sprintf((char*)buf, "%X", mintilt);
00809   command[6] = buf[0];
00810   command[7] = buf[1];
00811   command[8] = buf[2];
00812   command[9] = buf[3];
00813   maxtilt = (int)(floor(MAX_TILT/.1125) + 0x8000);
00814   sprintf((char*)buf, "%X", maxtilt);
00815 
00816   command[10] = buf[0];
00817   command[11] = buf[1];
00818   command[12] = buf[2];
00819   command[13] = buf[3];
00820   command[14] = FOOTER;
00821 
00822   if(sendCommand(command, 15))
00823     return -1;
00824   if (bidirectional_com_)
00825   {
00826     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00827   }
00828   else
00829   {
00830     usleep(SLEEP_TIME_USEC);
00831     return 0;
00832   }
00833 
00834   // return(receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00835 
00836 }
00837 
00838 int P2OSPtz::getAbsPanTilt(int* pan, int* tilt)
00839 {
00840   unsigned char command[MAX_COMMAND_LENGTH];
00841   unsigned char reply[MAX_REQUEST_LENGTH];
00842   int reply_len;
00843   unsigned char buf[4];
00844   char byte;
00845   unsigned int u_val;
00846   int val;
00847   int i;
00848 
00849   command[0] = HEADER;
00850   command[1] = DEVICEID;
00851   command[2] = DEVICEID;
00852   command[3] = DELIM;
00853   command[4] = PANTILTREQ;
00854   command[5] = FOOTER;
00855 
00856   if (sendRequest(command, 6, reply))
00857     return(-1);
00858   if (bidirectional_com_)
00859   {
00860     reply_len = receiveRequestAnswer(reply,14,0);
00861   }
00862   else
00863   {
00864     return 0;
00865   }
00866 
00867   if ( reply_len != 14 ) {
00868     ROS_ERROR("Reply Len = %i; should equal 14", reply_len);
00869     return -1;
00870   }
00871 
00872   // remove the ascii encoding, and put into 4-byte array
00873   for (i = 0; i < 4; i++)
00874   {
00875     byte = reply[i+5];
00876     if (byte < 0x40)
00877       byte = byte - 0x30;
00878     else
00879       byte = byte - 'A' + 10;
00880     buf[i] = byte;
00881   }
00882 
00883   // convert the 4-bytes into a number
00884   u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
00885 
00886   // convert the number to a value that's meaningful, based on camera specs
00887   val = (int)(((int)u_val - (int)0x8000) * 0.1125);
00888 
00889   // now set myPan to the response received for where the camera thinks it is
00890   *pan = val;
00891 
00892   // repeat the steps for the tilt value
00893   for (i = 0; i < 4; i++)
00894   {
00895     byte = reply[i+9];
00896     if (byte < 0x40)
00897       byte = byte - 0x30;
00898     else
00899       byte = byte - 'A' + 10;
00900     buf[i] = byte;
00901   }
00902   u_val = buf[0] * 0x1000 + buf[1] * 0x100 + buf[2] * 0x10 + buf[3];
00903   val =(int)(((int)u_val  - (int)0x8000) * 0.1125);
00904   *tilt = val;
00905 
00906   return(0);
00907 }
00908 
00909 int P2OSPtz::sendAbsPanTilt(int pan, int tilt)
00910 {
00911   unsigned char command[MAX_COMMAND_LENGTH];
00912   int convpan, convtilt;
00913   unsigned char buf[5];
00914   int ppan, ttilt;
00915 
00916   ppan = pan; ttilt = tilt;
00917   if(pan < MIN_PAN)
00918   {
00919     ppan = (int)MIN_PAN;
00920   }
00921   else if(pan > MAX_PAN)
00922   {
00923       ppan = (int)MAX_PAN;
00924   }
00925 
00926   if (tilt > MAX_TILT)
00927   {
00928     ttilt = (int)MAX_TILT;
00929   }
00930   else if(tilt < MIN_TILT)
00931   {
00932       ttilt = (int)MIN_TILT;
00933   }
00934   //puts("Camera pan angle thresholded");
00935 
00936   //puts("Camera tilt angle thresholded");
00937 
00938   convpan = (int)floor(ppan/.1125) + 0x8000;
00939   convtilt = (int)floor(ttilt/.1125) + 0x8000;
00940   //   fprintf(stdout, "ppan: %d ttilt: %d conpan: %d contilt: %d\n",
00941   //      ppan,ttilt,convpan,convtilt);
00942   command[0] = HEADER;
00943   command[1] = DEVICEID;
00944   command[2] = DEVICEID;
00945   command[3] = DELIM;
00946   command[4] = PANTILT;
00947   // pan position
00948 
00949   snprintf((char *)buf,sizeof(buf), "%X", convpan);
00950 
00951   command[5] = buf[0];
00952   command[6] = buf[1];
00953   command[7] = buf[2];
00954   command[8] = buf[3];
00955   // tilt position
00956   snprintf((char *)buf,sizeof(buf), "%X", convtilt);
00957   command[9]  = buf[0];
00958   command[10] = buf[1];
00959   command[11] = buf[2];
00960   command[12] = buf[3];
00961   command[13] = (unsigned char) FOOTER;
00962   if(sendCommand(command, 14))
00963     return -1;
00964 
00965   tilt_ = ttilt;
00966   pan_ = ppan;
00967 
00968   if (bidirectional_com_)
00969   {
00970     return (receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00971   }
00972   else
00973   {
00974     usleep(SLEEP_TIME_USEC);
00975     return 0;
00976   }
00977 
00978   //return(receiveCommandAnswer(COMMAND_RESPONSE_BYTES));
00979 }
00980 
00981 //
00982 // Circular Buffer To deal with getting data back from AUX
00983 //
00984 circbuf::circbuf(int size) :
00985     start(0), end(0), mysize(size), gotPack(false)
00986 {
00987   this->buf = new unsigned char[size];
00988 }
00989 
00990 void circbuf::printBuf(){
00991   int i = start;
00992   printf("circbuf: ");
00993   while ( i != end ){
00994     printf("0x%x ", buf[i]);
00995     i = (i+1)%mysize;
00996   }
00997   printf("\n");
00998 }
00999 
01000 
01001 void circbuf::putOnBuf(unsigned char c)
01002 {
01003   buf[end] = c;
01004   end = (end+1)%mysize;
01005   if ( end == start )
01006   {
01007     // We're overwriting old data.
01008     start = (start + 1)%mysize;
01009   }
01010 
01011   // Check to see if we have the whole packet now. (ends with FOOTER)
01012   if ( c == P2OSPtz::FOOTER )
01013   {
01014     gotPack = true;
01015   }
01016 }
01017 
01018 bool circbuf::haveData()
01019 {
01020   return !(this->start == this->end);
01021 }
01022 
01023 int circbuf::getFromBuf()
01024 {
01025   if ( start != end ){
01026     unsigned char ret = buf[start];
01027     start = (start+1)%mysize;
01028     return (int)ret;
01029   }
01030   else
01031   {
01032     return -1;
01033   }
01034 }
01035 
01036 int circbuf::size()
01037 {
01038   if ( end > start )
01039   {
01040     return end-start;
01041   }
01042   else if ( start > end )
01043   {
01044     return mysize - start - end - 1;
01045   }
01046   else
01047   {
01048     return 0;
01049   }
01050 }
01051 
01052 bool circbuf::gotPacket()
01053 {
01054   return gotPack;
01055 }
01056 
01057 void circbuf::reset()
01058 {
01059   memset(buf, 0, mysize);
01060   gotPack = false;
01061   start = end = 0;
01062 }


p2os_driver
Author(s): Hunter L. Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Sat Aug 5 2017 02:20:17