[Robotflow-cvs] RobotFlow/Devices/src SNCRZ30.cc,1.16,1.17

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/robotflow/RobotFlow/Devices/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv17931/src

Modified Files:
	SNCRZ30.cc 
Log Message:
inquiry camera position when required

Index: SNCRZ30.cc
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30.cc,v
retrieving revision 1.16
retrieving revision 1.17
diff -C2 -d -r1.16 -r1.17
*** SNCRZ30.cc	26 Aug 2004 13:49:41 -0000	1.16
--- SNCRZ30.cc	26 Aug 2004 20:05:48 -0000	1.17
***************
*** 81,85 ****
   * @parameter_value false
   * @parameter_description Wait for camera reply on commands
! 
   * @output_name PAN_POSITION
   * @output_type int
--- 81,90 ----
   * @parameter_value false
   * @parameter_description Wait for camera reply on commands
!  *
!  * @parameter_name INQUIRY_POSITION
!  * @parameter_type bool
!  * @parameter_value false
!  * @parameter_description Inquiry position of the pan,tilt,zoom from the device
!  *
   * @output_name PAN_POSITION
   * @output_type int
***************
*** 108,112 ****
  
  SNCRZ30::SNCRZ30(string nodeName, ParameterSet params)
!   : BufferedNode(nodeName,params), m_port(80), m_imageSocket(network_socket::TCP_STREAM_TYPE,80), m_dataSocket(network_socket::TCP_STREAM_TYPE,80),m_image(320,240,3) {
  
      //inputs
--- 113,117 ----
  
  SNCRZ30::SNCRZ30(string nodeName, ParameterSet params)
!   : BufferedNode(nodeName,params), m_port(80), m_imageSocket(network_socket::TCP_STREAM_TYPE,80), m_dataSocket(network_socket::TCP_STREAM_TYPE,80),m_image(320,240,3), m_inquiryPosition(false) {
  
      //inputs
***************
*** 130,133 ****
--- 135,139 ----
      m_continuous = dereference_cast<bool>(parameters.get("CONTINUOUS"));
      m_wait_reply = dereference_cast<bool>(parameters.get("WAIT_REPLY"));
+     m_inquiryPosition = dereference_cast<bool>(parameters.get("INQUIRY_POSITION"));
  
      //initialize camera position
***************
*** 165,168 ****
--- 171,179 ----
    //or single grab an image
    //special case when we want to output the image, we do not need to pull inputs
+ 
+   //int pan, tilt,zoom;
+   //inquiry_pan_tilt(pan,tilt);
+   //inquiry_zoom(zoom);
+ 
    if (output_id == m_imageOutID) {
      if (m_continuous) {
***************
*** 183,186 ****
--- 194,202 ----
    else {
  
+     if (m_inquiryPosition) {
+       inquiry_pan_tilt(m_currentPan,m_currentTilt);
+       inquiry_zoom(m_currentZoom);
+     }
+ 
      //process all other inputs/outputs
  
***************
*** 198,203 ****
        RCPtr<Int> tilt = RelTiltValue;
        send_pan_tilt_relative_position(m_panSpeed,*pan,m_tiltSpeed, *tilt);
!       m_currentPan+=*pan;
!       m_currentTilt+=*tilt;
      }
      else {
--- 214,221 ----
        RCPtr<Int> tilt = RelTiltValue;
        send_pan_tilt_relative_position(m_panSpeed,*pan,m_tiltSpeed, *tilt);
!       if (!m_inquiryPosition) {
! 	m_currentPan+=*pan;
! 	m_currentTilt+=*tilt;
!       }
      }
      else {
***************
*** 205,209 ****
  	RCPtr<Int> pan = RelPanValue;
  	send_pan_tilt_relative_position(m_panSpeed,*pan,m_tiltSpeed, 0);
! 	m_currentPan+=*pan;
        }
        
--- 223,229 ----
  	RCPtr<Int> pan = RelPanValue;
  	send_pan_tilt_relative_position(m_panSpeed,*pan,m_tiltSpeed, 0);
! 	if (!m_inquiryPosition) {
! 	  m_currentPan+=*pan;
! 	}
        }
        
***************
*** 211,215 ****
  	RCPtr<Int> tilt = RelTiltValue;
  	send_pan_tilt_relative_position(m_panSpeed,0,m_tiltSpeed,*tilt);
! 	m_currentTilt+=*tilt;
        }
      }
--- 231,237 ----
  	RCPtr<Int> tilt = RelTiltValue;
  	send_pan_tilt_relative_position(m_panSpeed,0,m_tiltSpeed,*tilt);
! 	if (!m_inquiryPosition) {
! 	  m_currentTilt+=*tilt;
! 	}
        }
      }
***************
*** 219,224 ****
        RCPtr<Int> tilt = AbsTiltValue;
        send_pan_tilt_absolute_position(m_panSpeed,*pan,m_tiltSpeed, *tilt);
!       m_currentPan =  *pan;
!       m_currentTilt = *tilt;
      }
      else {
--- 241,248 ----
        RCPtr<Int> tilt = AbsTiltValue;
        send_pan_tilt_absolute_position(m_panSpeed,*pan,m_tiltSpeed, *tilt);
!       if (!m_inquiryPosition) {
! 	m_currentPan =  *pan;
! 	m_currentTilt = *tilt;
!       }
      }
      else {
***************
*** 226,230 ****
  	RCPtr<Int> pan = AbsPanValue;
  	send_pan_tilt_absolute_position(m_panSpeed,*pan,m_tiltSpeed, m_currentTilt);
! 	m_currentPan = *pan;
        }
        
--- 250,256 ----
  	RCPtr<Int> pan = AbsPanValue;
  	send_pan_tilt_absolute_position(m_panSpeed,*pan,m_tiltSpeed, m_currentTilt);
! 	if (!m_inquiryPosition) {
! 	  m_currentPan = *pan;
! 	}
        }
        
***************
*** 232,236 ****
  	RCPtr<Int> tilt = AbsTiltValue;
  	send_pan_tilt_absolute_position(m_panSpeed,m_currentPan,m_tiltSpeed, *tilt);
! 	m_currentTilt = *tilt;
        } 
      }
--- 258,264 ----
  	RCPtr<Int> tilt = AbsTiltValue;
  	send_pan_tilt_absolute_position(m_panSpeed,m_currentPan,m_tiltSpeed, *tilt);
! 	if (!m_inquiryPosition) {
! 	  m_currentTilt = *tilt;
! 	}
        } 
      }
***************
*** 239,250 ****
        RCPtr<Int> zoom = AbsZoomValue;
        send_zoom_position(*zoom);
!       m_currentZoom = *zoom;
      }   
  
      if (!RelZoomValue->isNil()) {
        RCPtr<Int> zoom = RelZoomValue;
!       m_currentZoom += *zoom;
!       send_zoom_position(m_currentZoom);
!     }
  
      //limit pan,tilt,zoom
--- 267,282 ----
        RCPtr<Int> zoom = AbsZoomValue;
        send_zoom_position(*zoom);
!       if (!m_inquiryPosition) {
! 	m_currentZoom = *zoom;
!       }
      }   
  
      if (!RelZoomValue->isNil()) {
        RCPtr<Int> zoom = RelZoomValue;
!       send_zoom_position(m_currentZoom + *zoom);
!       if (!m_inquiryPosition) {
! 	m_currentZoom += *zoom;
!       }
!    }
  
      //limit pan,tilt,zoom
***************
*** 286,291 ****
     my_stream << "Connection: Keep-Alive\r\n";
     my_stream << "Cache-Control: no-cache\r\n";
!    my_stream << "Content-Length: 16\r\n\r\n";
!    my_stream << "VISCA=81010605FF";
  
     //cerr.write(my_stream.str().c_str(),my_stream.str().size());
--- 318,323 ----
     my_stream << "Connection: Keep-Alive\r\n";
     my_stream << "Cache-Control: no-cache\r\n";
!    my_stream << "Content-Length: 16\r\n";
!    my_stream << "VISCA=81010605FF\r\n";
  
     //cerr.write(my_stream.str().c_str(),my_stream.str().size());
***************
*** 320,324 ****
     my_stream << "Cache-Control: no-cache\r\n";
     my_stream << "Content-Length: 16\r\n\r\n";
!    my_stream << "VISCA=81010604FF";
  
  
--- 352,356 ----
     my_stream << "Cache-Control: no-cache\r\n";
     my_stream << "Content-Length: 16\r\n\r\n";
!    my_stream << "VISCA=81010604FF\r\n";
  
  
***************
*** 698,702 ****
    //create single grab command
    ostringstream my_stream;
!   my_stream << "GET http://"<<m_host<<":"<<m_port<<"/oneshotimage.jpg HTTP/1.1\r\n\r\n";
    
    //cerr.write(my_stream.str().c_str(),my_stream.str().size());
--- 730,736 ----
    //create single grab command
    ostringstream my_stream;
!   my_stream << "GET http://"<<m_host<<":"<<m_port<<"/oneshotimage.jpg HTTP/1.1";
!   //end message
!   my_stream << "\r\n\r\n";
    
    //cerr.write(my_stream.str().c_str(),my_stream.str().size());
***************
*** 739,744 ****
    my_stream << "Cache-Control: no-cache\r\n";
    my_stream << "Content-Length: 18\r\n\r\n";
!   my_stream << "VISCA=8101062403FF"; //auto-pan-tilt off
!   my_stream << "\r\n"; //end message
  
    //cerr.write(my_stream.str().c_str(),my_stream.str().size());
--- 773,778 ----
    my_stream << "Cache-Control: no-cache\r\n";
    my_stream << "Content-Length: 18\r\n\r\n";
!   my_stream << "VISCA=8101062403FF\r\n"; //auto-pan-tilt off
!  
  
    //cerr.write(my_stream.str().c_str(),my_stream.str().size());
***************
*** 759,760 ****
--- 793,911 ----
    }
  }
+ 
+ void SNCRZ30::inquiry_pan_tilt(int &pan_position, int &tilt_position) {
+ 
+ 
+   network_socket my_socket(network_socket::TCP_STREAM_TYPE,m_port);
+   my_socket.socket_connect(m_host.c_str());
+ 
+   //create focus command
+   ostringstream my_stream;
+   my_stream << "POST http://"<<m_host<<":"<<m_port<<"/command/visca-inquiry.cgi HTTP/1.1\r\n";
+   my_stream << "Connection: Keep-Alive\r\n";
+   my_stream << "Cache-Control: no-cache\r\n";
+   my_stream << "Content-Length: 16\r\n\r\n";
+   my_stream << "VISCA=81090612FF\r\n"; //pan-tilt inquiry 
+ 
+   //cerr.write(my_stream.str().c_str(),my_stream.str().size());
+ 
+   //send data
+   int sent_size =  my_socket.send_packet((unsigned char*)my_stream.str().c_str(), my_stream.str().size());  
+   
+   //get reply
+  
+   unsigned char buffer[228]; //1k buffer
+   int recv_size = my_socket.recv_packet(buffer,228);
+   //cerr<<"received size "<<recv_size<<endl;
+   
+   
+   //print reply
+   //for (int i = 0; i < recv_size; i++) {
+   //  cerr<<i<<" "<<buffer[i]<<" "<<endl;;
+   //}
+   
+   if (recv_size == 228) {
+     stringstream pan_stream;
+     stringstream tilt_stream;
+ 
+     pan_stream.setf ( ios_base::hex, ios_base::basefield );
+     tilt_stream.setf ( ios_base::hex, ios_base::basefield );
+ 
+     //pan_stream.write("0x",2);
+     pan_stream.write((const char*) &buffer[211],1);
+     pan_stream.write((const char*) &buffer[213],1);
+     pan_stream.write((const char*) &buffer[215],1);
+     pan_stream.write((const char*) &buffer[217],1);
+     
+     //tilt_stream.write("0x",2);
+     tilt_stream.write((const char*) &buffer[219],1);
+     tilt_stream.write((const char*) &buffer[221],1);
+     tilt_stream.write((const char*) &buffer[223],1);
+     tilt_stream.write((const char*) &buffer[225],1);
+ 
+     int tmp_pan;
+     int tmp_tilt;
+     
+     pan_stream>>std::hex>>tmp_pan;
+     tilt_stream>>std::hex>>tmp_tilt;
+ 
+     //strange, we have to do that conversion since the input hex input does not 
+     //handle negative data properly
+     pan_position = (short)tmp_pan;
+     tilt_position = (short)tmp_tilt;    
+   }
+ 
+ 
+ }
+ 
+ void SNCRZ30::inquiry_zoom(int &zoom_position) {
+ 
+ 
+   network_socket my_socket(network_socket::TCP_STREAM_TYPE,m_port);
+   my_socket.socket_connect(m_host.c_str());
+ 
+   //create focus command
+   ostringstream my_stream;
+   my_stream << "POST http://"<<m_host<<":"<<m_port<<"/command/visca-inquiry.cgi HTTP/1.1\r\n";
+   my_stream << "Connection: Keep-Alive\r\n";
+   my_stream << "Cache-Control: no-cache\r\n";
+   my_stream << "Content-Length: 16\r\n\r\n";
+   my_stream << "VISCA=81090447FF\r\n"; //pan-tilt inquiry 
+ 
+   //cerr.write(my_stream.str().c_str(),my_stream.str().size());
+ 
+   //send data
+   int sent_size =  my_socket.send_packet((unsigned char*)my_stream.str().c_str(), my_stream.str().size());  
+   
+   //get reply
+  
+   unsigned char buffer[220]; //1k buffer
+   int recv_size = my_socket.recv_packet(buffer,220);
+   //cerr<<"zoom received size "<<recv_size<<endl;
+   
+   //print reply
+   //for (int i = 0; i < recv_size; i++) {
+   //  cerr<<i<<" "<<buffer[i]<<" "<<endl;;
+   //}
+   
+   if (recv_size == 220) {
+ 
+     stringstream zoom_stream;
+     zoom_stream.setf ( ios_base::hex, ios_base::basefield );
+   
+     //pan_stream.write("0x",2);
+     zoom_stream.write((const char*) &buffer[211],1);
+     zoom_stream.write((const char*) &buffer[213],1);
+     zoom_stream.write((const char*) &buffer[215],1);
+     zoom_stream.write((const char*) &buffer[217],1);
+      
+     int tmp_zoom;
+    
+     zoom_stream>>std::hex>>tmp_zoom;
+   
+     //strange, we have to do that conversion since the input hex input does not 
+     //handle negative data properly
+     zoom_position = (short)tmp_zoom;
+     //cerr<<"zoom position : "<<zoom_position;
+   }
+ }