acts.cc

机器人仿真平台,和stage配合运行

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *                      
 *
 *  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: acts.cc,v 1.1.2.1 2003/02/13 20:01:39 gerkey Exp $
 *
 *   the P2 vision device.  it takes pan tilt zoom commands for the
 *   sony PTZ camera (if equipped), and returns color blob data gathered
 *   from ACTS, which this device spawns and then talks to.
 */

#if HAVE_CONFIG_H
  #include <config.h>
#endif

#if HAVE_STRINGS_H
  #include <strings.h>
#endif

#include <assert.h>
#include <stdio.h>
#include <unistd.h> /* close(2),fcntl(2),getpid(2),usleep(3),execvp(3),fork(2)*/
#include <netdb.h> /* for gethostbyname(3) */
#include <netinet/in.h>  /* for struct sockaddr_in, htons(3) */
#include <sys/types.h>  /* for socket(2) */
#include <sys/socket.h>  /* for socket(2) */
#include <signal.h>  /* for kill(2) */
#include <fcntl.h>  /* for fcntl(2) */
#include <string.h>  /* for strncpy(3),memcpy(3) */
#include <stdlib.h>  /* for atexit(3),atoi(3) */
#include <pthread.h>  /* for pthread stuff */
#include <socket_util.h>

#include <drivertable.h>
#include <player.h>

#define ACTS_NUM_CHANNELS 32
#define ACTS_HEADER_SIZE_1_0 2*ACTS_NUM_CHANNELS  
#define ACTS_HEADER_SIZE_1_2 4*ACTS_NUM_CHANNELS  
#define ACTS_BLOB_SIZE_1_0 10
#define ACTS_BLOB_SIZE_1_2 16
#define ACTS_MAX_BLOBS_PER_CHANNEL 10

#define ACTS_VERSION_1_0_STRING "1.0"
#define ACTS_VERSION_1_2_STRING "1.2"
#define ACTS_VERSION_2_0_STRING "2.0"

#define DEFAULT_ACTS_PORT 5001

/* a variable of this type tells the vision device how to interact with ACTS */
typedef enum
{
  ACTS_VERSION_UNKNOWN = 0,
  ACTS_VERSION_1_0 = 1,
  ACTS_VERSION_1_2 = 2,
  ACTS_VERSION_2_0 = 3
} acts_version_t;
/* default is to use older ACTS (until we change our robots) */
#define DEFAULT_ACTS_VERSION ACTS_VERSION_1_0
#define DEFAULT_ACTS_CONFIGFILE "/usr/local/acts/actsconfig"
/* default is to give no path for the binary; in this case, use execvp() 
 * and user's PATH */
#define DEFAULT_ACTS_PATH ""
#define DEFAULT_ACTS_WIDTH 160
#define DEFAULT_ACTS_HEIGHT 120
/********************************************************************/

class Acts:public CDevice 
{
  private:
    int debuglevel;             // debuglevel 0=none, 1=basic, 2=everything
    int pid;      // ACTS's pid so we can kill it later

    
    // returns the enum representation of the given version string, or
    // ACTS_VERSION_UNKNOWN on failure to match.
    acts_version_t version_string_to_enum(char* versionstr);
    
    // writes the string representation of the given version number into
    // versionstr, up to len.
    // returns  0 on success
    //         -1 on failure to match.
    int version_enum_to_string(acts_version_t versionnum, char* versionstr, 
                               int len);

    // stuff that will be used on the cmdline to start ACTS
    acts_version_t acts_version;  // the ACTS version, as an enum
    char binarypath[MAX_FILENAME_SIZE];  // path to executable
    char configfilepath[MAX_FILENAME_SIZE];  // path to configfile (-t)
    char minarea[8];  // min num of pixels for tracking (-w)
    int portnum;  // port number where we'll connect to ACTS (-p)
    char fps[8]; // capture rate (-R)
    char drivertype[8]; // e.g., bttv, bt848 (-G)
    char invertp; // invert the image? (-i)
    char devicepath[MAX_FILENAME_SIZE]; // e.g., /dev/video0 (-d)
    char channel[8]; // channel to use (-n)
    char norm[8]; // PAL or NTSC (-V)
    char pxc200p; // using PXC200? (-x)
    char brightness[8]; // brightness of image (-B)
    char contrast[8]; // contrast of image (-C)
    int width, height;  // the image dimensions. (-W, -H)

    int header_len; // length of incoming packet header (varies by version)
    int header_elt_len; // length of each header element (varies by version)
    int blob_size;  // size of each incoming blob (varies by version)
    char portnumstring[128]; // string version of portnum
    char widthstring[128];
    char heightstring[128];

    // Descriptive colors for each channel.
    uint32_t colors[PLAYER_BLOBFINDER_MAX_CHANNELS];

  public:
    int sock;               // socket to ACTS

    // constructor 
    //
    Acts(char* interface, ConfigFile* cf, int section);

    virtual void Main();

    void KillACTS();

    int Setup();
    int Shutdown();
};

// a factory creation function
CDevice* Acts_Init(char* interface, ConfigFile* cf, int section)
{
  if(strcmp(interface, PLAYER_BLOBFINDER_STRING))
  {
    PLAYER_ERROR1("driver \"acts\" does not support interface \"%s\"\n",
                  interface);
    return(NULL);
  }
  else
    return((CDevice*)(new Acts(interface, cf, section)));
}

// a driver registration function
void 
Acts_Register(DriverTable* table)
{
  table->AddDriver("acts", PLAYER_READ_MODE, Acts_Init);
}

#define ACTS_REQUEST_QUIT '1'
#define ACTS_REQUEST_PACKET '0'

/* the following setting mean that we first try to connect after 1 seconds,
 * then try every 100ms for 6 more seconds before giving up */
#define ACTS_STARTUP_USEC 1000000 /* wait before first connection attempt */
#define ACTS_STARTUP_INTERVAL_USEC 100000 /* wait between connection attempts */
#define ACTS_STARTUP_CONN_LIMIT 60 /* number of attempts to make */


void QuitACTS(void* visiondevice);


Acts::Acts(char* interface, ConfigFile* cf, int section)
  : CDevice(sizeof(player_blobfinder_data_t),0,0,0)
{
  char tmpstr[MAX_FILENAME_SIZE];
  int tmpint;
  double tmpfloat;
  int ch;
  uint32_t color;

  sock = -1;

  // first, get the necessary args
  strncpy(binarypath,
          cf->ReadFilename(section, "path", DEFAULT_ACTS_PATH),
          sizeof(binarypath));
  strncpy(configfilepath, 
          cf->ReadFilename(section, "configfile", DEFAULT_ACTS_CONFIGFILE),
          sizeof(configfilepath));
  strncpy(tmpstr,
          cf->ReadString(section, "version", ACTS_VERSION_2_0_STRING),
          sizeof(tmpstr));
  if((acts_version = version_string_to_enum(tmpstr)) == ACTS_VERSION_UNKNOWN)
  {
    PLAYER_WARN2("unknown version \"%s\"; using default \"%s\"",
                 tmpstr, ACTS_VERSION_1_0_STRING);
    acts_version = version_string_to_enum(ACTS_VERSION_2_0_STRING);
  }
  width = cf->ReadInt(section, "width", DEFAULT_ACTS_WIDTH);
  height = cf->ReadInt(section, "height", DEFAULT_ACTS_HEIGHT);
  
  // now, get the optionals
  bzero(minarea,sizeof(minarea));
  if((tmpint = cf->ReadInt(section, "pixels", -1)) >= 0)
    snprintf(minarea,sizeof(minarea),"%d",tmpint);
  portnum = cf->ReadInt(section, "port", DEFAULT_ACTS_PORT);
  bzero(fps,sizeof(fps));
  if((tmpint = cf->ReadInt(section, "fps", -1)) >= 0)
    snprintf(fps,sizeof(fps),"%d",tmpint);
  bzero(drivertype,sizeof(drivertype));
  if(cf->ReadString(section, "drivertype", NULL))
    strncpy(drivertype, cf->ReadString(section, "drivertype", NULL), 
            sizeof(drivertype)-1);
  invertp = cf->ReadInt(section, "invert", -1);
  bzero(devicepath,sizeof(devicepath));
  if(cf->ReadString(section, "devicepath", NULL))
    strncpy(devicepath, cf->ReadString(section, "devicepath", NULL), 
            sizeof(devicepath)-1);
  bzero(channel,sizeof(channel));
  if((tmpint = cf->ReadInt(section, "channel", -1)) >= 0)
    snprintf(channel,sizeof(channel),"%d",tmpint);
  bzero(norm,sizeof(norm));
  if(cf->ReadString(section, "norm", NULL))
    strncpy(norm, cf->ReadString(section, "norm", NULL), 
            sizeof(norm)-1);
  pxc200p = cf->ReadInt(section, "pxc200", -1);
  bzero(brightness,sizeof(brightness));
  if((tmpfloat = cf->ReadFloat(section, "brightness", -1)) >= 0)
    snprintf(brightness,sizeof(brightness),"%.3f",tmpfloat);
  bzero(contrast,sizeof(contrast));
  if((tmpfloat = cf->ReadFloat(section, "contrast", -1)) >= 0)
    snprintf(contrast,sizeof(brightness),"%.3f",tmpfloat);

  // set up some version-specific parameters
  switch(acts_version)
  {
    case ACTS_VERSION_1_0:
      header_len = ACTS_HEADER_SIZE_1_0;
      blob_size = ACTS_BLOB_SIZE_1_0;
      // extra byte-swap cause ACTS 1.0 got it wrong
      portnum = htons(portnum);
      break;
    case ACTS_VERSION_1_2:
    case ACTS_VERSION_2_0:
    default:
      header_len = ACTS_HEADER_SIZE_1_2;
      blob_size = ACTS_BLOB_SIZE_1_2;
      break;
  }
  header_elt_len = header_len / PLAYER_BLOBFINDER_MAX_CHANNELS;
  bzero(portnumstring, sizeof(portnumstring));
  snprintf(portnumstring,sizeof(portnumstring),"%d",portnum);
  
  bzero(widthstring, sizeof(widthstring));
  snprintf(widthstring,sizeof(widthstring),"%d",width);

  bzero(heightstring, sizeof(heightstring));
  snprintf(heightstring,sizeof(heightstring),"%d",height);


  // Get the descriptive colors.
  for (ch = 0; ch < PLAYER_BLOBFINDER_MAX_CHANNELS; ch++)
  {
    color = cf->ReadTupleColor(section, "colors", ch, 0xFFFFFFFF);
    if (color == 0xFFFFFFFF)
      break;
    this->colors[ch] = color;
  }
}
    
// returns the enum representation of the given version string, or
// -1 on failure to match.
acts_version_t Acts::version_string_to_enum(char* versionstr)
{
  if(!strcmp(versionstr,ACTS_VERSION_1_0_STRING))
    return(ACTS_VERSION_1_0);
  else if(!strcmp(versionstr,ACTS_VERSION_1_2_STRING))
    return(ACTS_VERSION_1_2);
  else if(!strcmp(versionstr,ACTS_VERSION_2_0_STRING))
    return(ACTS_VERSION_2_0);
  else
    return(ACTS_VERSION_UNKNOWN);
}

// writes the string representation of the given version number into
// versionstr, up to len.
// returns  0 on success
//         -1 on failure to match.
int Acts::version_enum_to_string(acts_version_t versionnum, 
                                          char* versionstr, int len)
{
  switch(versionnum)
  {
    case ACTS_VERSION_1_0:
      strncpy(versionstr, ACTS_VERSION_1_0_STRING, len);
      return(0);
    case ACTS_VERSION_1_2:
      strncpy(versionstr, ACTS_VERSION_1_2_STRING, len);
      return(0);
    case ACTS_VERSION_2_0:
      strncpy(versionstr, ACTS_VERSION_2_0_STRING, len);
      return(0);
    default:
      return(-1);
  }
}

int
Acts::Setup()
{
  int i;
  int j;

  char acts_bin_name[] = "acts";
  //char acts_configfile_flag[] = "-t";
  //char acts_port_flag[] = "-s";

  char* acts_args[32];

  static struct sockaddr_in server;
  char host[] = "localhost";
  struct hostent* entp;

  printf("ACTS vision server connection initializing...");
  fflush(stdout);

  player_blobfinder_data_t dummy;
  bzero(&dummy,sizeof(dummy));
  // zero the data buffer
  PutData((unsigned char*)&dummy,
          sizeof(dummy.width)+sizeof(dummy.height)+sizeof(dummy.header),0,0);

  i = 0;
  acts_args[i++] = acts_bin_name;
  // build the argument list, based on version
  switch(acts_version)
  {
    case ACTS_VERSION_1_0:
      acts_args[i++] = "-t";
      acts_args[i++] = configfilepath;
      if(strlen(portnumstring))
      {
        acts_args[i++] = "-s";
        acts_args[i++] = portnumstring;
      }
      if(strlen(devicepath))
      {
        acts_args[i++] = "-d";
        acts_args[i++] = devicepath;
      }
      break;
    case ACTS_VERSION_1_2:
      acts_args[i++] = "-t";
      acts_args[i++] = configfilepath;
      if(strlen(portnumstring))
      {
        acts_args[i++] = "-p";
        acts_args[i++] = portnumstring;
      }
      if(strlen(devicepath))
      {
        acts_args[i++] = "-d";
        acts_args[i++] = devicepath;
      }
      if(strlen(contrast))
      {
        acts_args[i++] = "-C";
        acts_args[i++] = contrast;
      }
      if(strlen(brightness))
      {
        acts_args[i++] = "-B";
        acts_args[i++] = brightness;
      }
      acts_args[i++] = "-W";
      acts_args[i++] = widthstring;
      acts_args[i++] = "-H";
      acts_args[i++] = heightstring;
      break;
    case ACTS_VERSION_2_0:
      acts_args[i++] = "-t";
      acts_args[i++] = configfilepath;
      if(strlen(minarea))
      {
        acts_args[i++] = "-w";
        acts_args[i++] = minarea;
      }
      if(strlen(portnumstring))
      {
        acts_args[i++] = "-p";
        acts_args[i++] = portnumstring;
      }
      if(strlen(fps))
      {
        acts_args[i++] = "-R";
        acts_args[i++] = fps;
      }
      if(strlen(drivertype))
      {
        acts_args[i++] = "-G";
        acts_args[i++] = drivertype;
      }
      if(invertp > 0)
        acts_args[i++] = "-i";
      if(strlen(devicepath))
      {
        acts_args[i++] = "-d";
        acts_args[i++] = devicepath;
      }
      if(strlen(channel))
      {
        acts_args[i++] = "-n";