acts.cc

机器人仿真软件

/*
 *  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.3.2.1 2006/06/07 16:12: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.
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_acts acts
 * @brief ActivMedia color tracking system

@todo This driver is currently disabled because it needs to be updated to
the Player 2.0 API.

ACTS is a fast color segmentation system written by Paul Rybski and sold
by <a href=http://www.activrobots.com>ActivMedia</a>.  After training,
ACTS finds colored blobs in a single camera image.  Player's acts driver
starts and controls ACTS, which must already be installed and trained.

The acts driver supports ACTS versions 1.0, 1.2, and 2.0.

PXC200 framegrabbers, when accessed through the bttv module, may cause
the machine to hang.  A workaround is to first read a frame from a
framegrabber channel on which there is no video signal, and then start
reading from the right channel.  (This problem is unrelated to Player's
acts driver).

@par Compile-time dependencies

- none

@par Provides

- @ref interface_blobfinder

@par Requires

- none

@par Configuration requests

- none

@par Configuration file options

In the list below, a default value of (none) indicates that the associated
option will not be passed to ACTS.  As a result, ACTS's own internal
default for that parameter will be used.  Consult the ACTS manual to
determine what those defaults are.

- path (string)
  - Default: ""
  - Path to the ACTS executable (leave empty to search the user's PATH
    for acts).
- configfile (string)
  - Default: "/usr/local/acts/actsconfig"
  - Path to the ACTS configuration file to be used.
- version (string)
  - Default: "2.0"
  - The version of ACTS in use (should be "1.0", "1.2", or "2.0")
- width (integer)
  - Default: 160
  - Width of the camera image (in pixels)
- height (integer)
  - Default: 120
  - Height of the camera image (in pixels)
- pixels (integer)
  - Default: (none)
  - Minimum area required to call a blob a blob (in pixels)
- port (integer)
  - Default: 5001
  - TCP port by which Player should connect to ACTS.
- fps (integer)
  - Default: (none)
  - Frame per second of the camera.
- drivertype (string)
  - Default: (none)
  - Type of framegrabber driver in use (e.g., "bttv", "bt848", "matrox").
- invert (integer)
  - Default: (none)
  - Is the camera inverted?
- devicepath (string)
  - Default: (none)
  - Path to the device file for the framegrabber (e.g., "/dev/fg0")
- channel (integer)
  - Default: (none)
  - Which channel to select on the framegrabber.
- norm (string)
  - Default: (none)
  - Something to do with normalization
- pxc200 (integer)
  - Default: (none)
  - Is the framegrabber a PXC200?
- brightness (float)
  - Default: (none)
  - Brightness level
- contrast (float)
   - Default: (none)
   - Contrast level

@par Example

@verbatim
driver
(
  name "acts"
  provides ["blobfinder:0"]
  configfile "/tmp/myactsconfig"
)
@endverbatim

@author Brian Gerkey

*/

/** @} */

#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 <libplayercore/playercore.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
/********************************************************************/


/** The maximum number of unique color classes. */
#define ACTS_MAX_CHANNELS 32

/** The maximum number of blobs for each color class. */
#define ACTS_MAX_BLOBS_PER_CHANNEL 10

/** The maximum number of blobs in total. */
#define ACTS_MAX_BLOBS ACTS_MAX_CHANNELS * ACTS_MAX_BLOBS_PER_CHANNEL

/** Blob index entry. */
typedef struct acts_header_elt
{
  /** Offset of the first blob for this channel. */
  uint16_t index;

  /** Number of blobs for this channel. */
  uint16_t num;
  
} acts_header_elt_t;


/** Structure describing a single blob. */
typedef struct acts_blob_elt
{
  /** A descriptive color for the blob (useful for gui's).  The color
      is stored as packed 32-bit RGB, i.e., 0x00RRGGBB. */
  uint32_t color;

  /** The blob area (pixels). */
  uint32_t area;

  /** The blob centroid (image coords). */
  uint16_t x, y;

  /** Bounding box for the blob (image coords). */
  uint16_t left, right, top, bottom;
  
} acts_blob_elt_t;


/** The list of detected blobs. */
typedef struct acts_data
{
  /** The image dimensions. */
  uint16_t width, height;

  /** An index into the list of blobs (blobs are indexed by channel). */
  acts_header_elt_t header[ACTS_MAX_CHANNELS];

  /** The list of blobs. */
  acts_blob_elt_t blobs[ACTS_MAX_BLOBS];
  
} acts_data_t;



class Acts:public Driver 
{
  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[ACTS_MAX_CHANNELS];

  public:
    int sock;               // socket to ACTS

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

    virtual void Main();

    void KillACTS();

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

// a factory creation function
Driver* Acts_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new Acts( cf, section)));
}

// a driver registration function
void 
Acts_Register(DriverTable* table)
{
  table->AddDriver("acts", 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( ConfigFile* cf, int section)
  : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_BLOBFINDER_CODE)
{
  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
  memset(minarea,0,sizeof(minarea));
  if((tmpint = cf->ReadInt(section, "pixels", -1)) >= 0)
    snprintf(minarea,sizeof(minarea),"%d",tmpint);
  portnum = cf->ReadInt(section, "port", DEFAULT_ACTS_PORT);
  memset(fps,0,sizeof(fps));
  if((tmpint = cf->ReadInt(section, "fps", -1)) >= 0)
    snprintf(fps,sizeof(fps),"%d",tmpint);
  memset(drivertype,0,sizeof(drivertype));
  if(cf->ReadString(section, "drivertype", NULL))
    strncpy(drivertype, cf->ReadString(section, "drivertype", NULL), 
            sizeof(drivertype)-1);
  invertp = cf->ReadInt(section, "invert", -1);
  memset(devicepath,0,sizeof(devicepath));
  if(cf->ReadString(section, "devicepath", NULL))
    strncpy(devicepath, cf->ReadString(section, "devicepath", NULL), 
            sizeof(devicepath)-1);
  memset(channel,0,sizeof(channel));
  if((tmpint = cf->ReadInt(section, "channel", -1)) >= 0)
    snprintf(channel,sizeof(channel),"%d",tmpint);
  memset(norm,0,sizeof(norm));
  if(cf->ReadString(section, "norm", NULL))
    strncpy(norm, cf->ReadString(section, "norm", NULL), 
            sizeof(norm)-1);
  pxc200p = cf->ReadInt(section, "pxc200", -1);
  memset(brightness,0,sizeof(brightness));
  if((tmpfloat = cf->ReadFloat(section, "brightness", -1)) >= 0)
    snprintf(brightness,sizeof(brightness),"%.3f",tmpfloat);
  memset(contrast,0,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 / ACTS_MAX_CHANNELS;
  memset(portnumstring, 0, sizeof(portnumstring));
  snprintf(portnumstring,sizeof(portnumstring),"%d",portnum);
  
  memset(widthstring, 0, sizeof(widthstring));
  snprintf(widthstring,sizeof(widthstring),"%d",width);

  memset(heightstring, 0, sizeof(heightstring));
  snprintf(heightstring,sizeof(heightstring),"%d",height);


  // Get the descriptive colors.
  for (ch = 0; ch < ACTS_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);

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

  i = 0;