sonyevid30.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * $Id: sonyevid30.cc,v 1.24 2006/02/27 18:19:03 gerkey Exp $
 *
 * methods for initializing, commanding, and getting data out of
 * the Sony EVI-D30 PTZ camera
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_sonyevid30 sonyevid30
 * @brief Sony EVI-D30 and EVI-D100 pan-tilt-zoom cameras

The sonyevid30 driver provides control of a Sony EVI-D30 and Sony EVI-D100
pan-tilt-zoom camera units.

The sonyevid30 driver operates over a direct serial link, not
through the P2OS microcontroller's AUX port, as is the normal
configuration for ActivMedia robots.  You may have to make or buy
a cable to connect your camera to a normal serial port.  Look <a
href="http://playerstage.sourceforge.net/faq.html#evid30_wiring">here</a>
for more information and wiring instructions.

The sonyevid30 driver only supports position control.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_ptz

@par Requires

- None

@par Configuration requests

- PLAYER_PTZ_GENERIC_CONFIG_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyS2"
  - The serial port to be used.

- fov (integer tuple)
  - Default: [3 30]
  - The minimum and maximum fields of view (in degrees), which will depend on
   the lens(es) you are using.  Half-angle??

- movement (integer)
  - Default: 0
  - Movement mode (?)
 
@par Example 

@verbatim
driver
(
  name "sonyevid30"
  provides ["ptz:0"]
  port "/dev/ttyS2"
  fov [3 30]
)
@endverbatim

@author Brian Gerkey, Brad Tonkes (D100 mode)

*/
/** @} */

#ifdef HAVE_CONFIG_H
  #include "config.h"
#endif

#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <termios.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>

#include <libplayercore/playercore.h>
#include <replace/replace.h>

#define MODEL_D3X 0x0402
#define MODEL_D100 0x040D

#define PTZ_SLEEP_TIME_USEC 100000

#define MAX_PTZ_PACKET_LENGTH 16
#define MAX_PTZ_MESSAGE_LENGTH 14
#define MAX_PTZ_REPLY_LENGTH 11

#define MAX_VER_MESSAGE_LENGTH 4
#define MAX_VER_REPLY_LENGTH 14

#define PTZ_PAN_MAX 100.0
#define PTZ_TILT_MAX 25.0

#define PTZ_MAX_PAN_SPEED	0x18
#define PTZ_MAX_TILT_SPEED	0x14

#define DEFAULT_PTZ_PORT "/dev/ttyS2"

#define VISCA_COMMAND_CODE	0x01
#define VISCA_INQUIRY_CODE	0x09

class SonyEVID30:public Driver 
{
 protected:
  bool command_pending1;  // keep track of how many commands are pending;
  bool command_pending2;  // that way, we can cancel them if necessary
  bool ptz_fd_blocking;
  
  // internal methods
  int Send(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1);
  int Receive(unsigned char* reply);
  int SendCommand(unsigned char* str, int len, uint8_t camera = 1);
  int CancelCommand(char socket);
  int SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1);
//  int HandleConfig(void *client, unsigned char *buf, size_t len);

  // MessageHandler
  int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data);

  // this function will be run in a separate thread
  virtual void Main();

  virtual int GetCameraType(int *model);

  virtual int SendAbsPanTilt(short pan, short tilt);
  virtual int SendStepPan(int);
  virtual int SendStepTilt(int);
  virtual int SendAbsZoom(short zoom);
  virtual int GetAbsZoom(short* zoom);
  virtual int GetAbsPanTilt(short* pan, short* tilt);
  virtual void PrintPacket(char* str, unsigned char* cmd, int len);

  double ptz_pan_conv_factor;
  double ptz_tilt_conv_factor;

 public:
  int ptz_fd; // ptz device file descriptor
  /* device used to communicate with the ptz */
  char ptz_serial_port[MAX_FILENAME_SIZE];

  // Min and max values for camera field of view (degrees).
  // These are used to compute appropriate zoom values.
  int maxfov, minfov;

protected:
  struct pollfd read_pfd;

  int movement_mode;
  int pandemand;
  int tiltdemand;

public:

  SonyEVID30( ConfigFile* cf, int section);

  virtual int Setup();
  virtual int Shutdown();
};
  
// initialization function
Driver* SonyEVID30_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new SonyEVID30( cf, section)));
}

/* how to make this work for multiple cameras...
   want to make a player device for each camera, ie ptz:0 ptz:1, so can read/write commands
   on the client side independently of how they are controlled.

   but for the sonys, sets of cameras are paritioned by serial port.  so
   we add a parameter "camera" to the config for ptz, and then here we have a table
   which keeps track of instantiations of devices according to serial port.
   
   will need to redo the class so that cameras on the same serial port share the port
   instead of each trying to open it.  they also have a port-id.  
   
   so _Init will read the config file and based on the serial port and camera parameter
   it will either create an instance of a serial-owning device, or instantiate a camera
   that shares an existing port.

   so SonyEVIController is the one that controls the port
   and create SonyEVIPeripheral that are the cameras.  each peripheral has an id that
   is given to create packets for that peripheral.  

   problem is this makes broadcasting commands more difficult/less efficient.
   use the new Wait and GetAvailable to share the port...
*/

// a driver registration function
void 
SonyEVID30_Register(DriverTable* table)
{
  table->AddDriver("sonyevid30",  SonyEVID30_Init);
}

SonyEVID30::SonyEVID30( ConfigFile* cf, int section) 
: Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_PTZ_CODE)
{
  ptz_fd = -1;
  command_pending1 = false;
  command_pending2 = false;

  movement_mode = 0;
  pandemand = 0;
  tiltdemand=0;

  read_pfd.events = POLLIN;

  // TODO: check field of view values.
  this->minfov = (int) RTOD(cf->ReadTupleAngle(section, "fov", 0, DTOR(3)));
  this->maxfov = (int) RTOD(cf->ReadTupleAngle(section, "fov", 1, DTOR(30)));

  // Assume we've got a D3X
  this->ptz_pan_conv_factor = 0x0370 / (double) PTZ_PAN_MAX;
  this->ptz_tilt_conv_factor = 0x012C / (double) PTZ_TILT_MAX;

  movement_mode = (int) cf->ReadInt(section, "movement", 0);

  strncpy(ptz_serial_port,
          cf->ReadString(section, "port", DEFAULT_PTZ_PORT),
          sizeof(ptz_serial_port));
}

int 
SonyEVID30::Setup()
{
  struct termios term;
  short pan,tilt;
  int cam_type;
  int flags;

  printf("PTZ connection initializing (%s)...", ptz_serial_port);
  fflush(stdout);

  // open it.  non-blocking at first, in case there's no ptz unit.
  if((ptz_fd = open(ptz_serial_port, O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
  {
    perror("SonyEVID30::Setup():open():");
    return(-1);
  }  

  read_pfd.fd = ptz_fd;

  if(tcflush(ptz_fd, TCIFLUSH ) < 0 )
  {
    perror("SonyEVID30::Setup():tcflush():");
    close(ptz_fd);
    ptz_fd = -1;
    return(-1);
  }
  if(tcgetattr(ptz_fd, &term) < 0 )
  {
    perror("SonyEVID30::Setup():tcgetattr():");
    close(ptz_fd);
    ptz_fd = -1;
    return(-1);
  }
  
  cfmakeraw(&term);
  cfsetispeed(&term, B9600);
  cfsetospeed(&term, B9600);
  
  if(tcsetattr(ptz_fd, TCSAFLUSH, &term) < 0 )
  {
    perror("SonyEVID30::Setup():tcsetattr():");
    close(ptz_fd);
    ptz_fd = -1;
    return(-1);
  }

	/* Work out what version of camera we are: d3x or d100.  The parameters of
	 * each are slightly different.
	 */
	if (GetCameraType(&cam_type)) {
		printf("Couldn't connect to PTZ device most likely because the camera\n"
				"is not connected or is connected not to %s\n", 
				ptz_serial_port);
		close(ptz_fd);
		ptz_fd = -1;
		return -1;
	} else {
		/* Here we calculate our conversion factors.
		 * The D3X series has a tilt range of +/- 25.0 degrees corresponding
		 * with +/- 0x12C, and a pan range of +/- 100.0 degrees corresponding
		 * with +/- 0x370.
		 * The D100 series has the same physical pan and tilt ranges, but
		 * represents these as +/- 0x168 (tilt) and 0x5A0 (pan).
		 */
		switch (cam_type) {
		case MODEL_D3X:
			this->ptz_pan_conv_factor = 0x0370 / (double) PTZ_PAN_MAX;
			this->ptz_tilt_conv_factor = 0x012C / (double) PTZ_TILT_MAX;
			break;
		case MODEL_D100:
			this->ptz_pan_conv_factor = 0x05A0 / (double) PTZ_PAN_MAX;
			this->ptz_tilt_conv_factor = 0x0168 / (double) PTZ_TILT_MAX;
			break;
		default:
			printf("Unknown camera type: %d\n", cam_type);
			break;
		}
	}

  ptz_fd_blocking = false;
  /* try to get current state, just to make sure we actually have a camera */
  if(GetAbsPanTilt(&pan,&tilt))
  {
    printf("Couldn't connect to PTZ device most likely because the camera\n"
                    "is not connected or is connected not to %s\n", 
                    ptz_serial_port);
    close(ptz_fd);
    ptz_fd = -1;
    return(-1);
  }

  /* ok, we got data, so now set NONBLOCK, and continue */
  if((flags = fcntl(ptz_fd, F_GETFL)) < 0)
  {
    perror("SonyEVID30::Setup():fcntl()");
    close(ptz_fd);
    ptz_fd = -1;
    return(1);
  }
  if(fcntl(ptz_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
  {
    perror("SonyEVID30::Setup():fcntl()");
    close(ptz_fd);
    ptz_fd = -1;
    return(1);
  }
  ptz_fd_blocking = true;
  puts("Done.");

  // zero the command and data buffers
//  player_ptz_data_t data;
//  player_ptz_cmd_t cmd;

//  data.pan = data.tilt = data.zoom = 0;
//  cmd.pan = cmd.tilt = 0;
//  cmd.zoom = this->maxfov;

//  PutData((void*)&data,sizeof(data),NULL);
//  PutCommand(this->device_id,(void*)&cmd,sizeof(cmd),NULL);

  // start the thread to talk with the camera
  StartThread();

  return(0);
}

int 
SonyEVID30::Shutdown()
{
  puts("SonyEVID30::Shutdown");

  if(ptz_fd == -1)
    return(0);

  StopThread();

  // put the camera back to center
  usleep(PTZ_SLEEP_TIME_USEC);
  SendAbsPanTilt(0,0);
  usleep(PTZ_SLEEP_TIME_USEC);
  SendAbsZoom(0);

  if(close(ptz_fd))
    perror("SonyEVID30::Shutdown():close():");
  ptz_fd = -1;
  puts("PTZ camera has been shutdown");
  return(0);
}

int
SonyEVID30::Send(unsigned char* str, int len, unsigned char* reply, uint8_t camera)
{
  unsigned char command[MAX_PTZ_PACKET_LENGTH];
  int i;

  if(len > MAX_PTZ_MESSAGE_LENGTH)
  {
    fprintf(stderr, "SonyEVID30::Send(): message is too large (%d bytes)\n",
                    len);
    return(-1);
  }

  assert(camera < 8);

  command[0] = 0x80 | camera; // controller address is 0, camera address 1
  for(i=0;i<len;i++)
    command[i+1] = str[i];

  command[i+1] = 0xFF;  // packet terminator

  //PrintPacket("Sending", command, i+2);
  
  // send the command
  if(write(ptz_fd, command, i+2) < 0)
  {
    perror("SonyEVID30::Send():write():");
    return(-1);
  }

  //puts("Send(): calling Receive()");
  return(Receive(reply));
}

int
SonyEVID30::Receive(unsigned char* reply)
{
  static unsigned char buffer[MAX_PTZ_PACKET_LENGTH];
  static int numread = 0;

  unsigned char temp_reply[MAX_PTZ_PACKET_LENGTH];
  int newnumread = 0;
  int bufptr = -1;
  int i;
  int temp;
  int pret;
  // if we're non-blocking, then we should wait a bit to give the
  // camera a chance to respond. 
  //  if(!ptz_fd_blocking)
  //    usleep(PTZ_SLEEP_TIME_USEC);


  
  memset(temp_reply,0,MAX_PTZ_PACKET_LENGTH);
  memset(reply,0,MAX_PTZ_PACKET_LENGTH);
  if(numread > 0)
  {
    //printf("copying %d old bytes\n", numread);
    memcpy(temp_reply,buffer,numread);
    // look for the terminator
    for(i=0;i<numread;i++)
    {
      if(temp_reply[i] == 0xFF)
      {
        bufptr = i;
        break;
      }
    }
  }

  while(bufptr < 0)
    {
      pret = poll(&read_pfd, 1, 1000);
      if (pret == 0) {
	printf("SONY: poll timedout !\n");
      } else if (pret < 0) {
	printf("SONY: poll returned error!\n");
      }
    newnumread = read(ptz_fd, temp_reply+numread, MAX_PTZ_REPLY_LENGTH-numread);
    if((numread += newnumread) < 0)
    {
      perror("SonyEVID30::Send():read():");
      return(-1);
    }
    else if(!newnumread)
    {
      // hmm...we were expecting something, yet we read
      // zero bytes. some glitch.  drain input, and return
      // zero.  we'll get a message next time through
      //puts("Receive(): read() returned 0");
      if(tcflush(ptz_fd, TCIFLUSH ) < 0 )
      {
        perror("SonyEVID30::Send():tcflush():");
        return(-1);
      }
      numread = 0;
      return(0);
    }
    // look for the terminator
    for(i=0;i<numread;i++)