acts.cc

机器人仿真软件

  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";
        acts_args[i++] = channel;
      }
      if(strlen(norm))
      {
        acts_args[i++] = "-V";
        acts_args[i++] = norm;
      }
      if(pxc200p > 0)
        acts_args[i++] = "-x";
      if(strlen(brightness))
      {
        acts_args[i++] = "-B";
        acts_args[i++] = brightness;
      }
      if(strlen(contrast))
      {
        acts_args[i++] = "-C";
        acts_args[i++] = contrast;
      }
      acts_args[i++] = "-W";
      acts_args[i++] = widthstring;
      acts_args[i++] = "-H";
      acts_args[i++] = heightstring;
      break;
    case ACTS_VERSION_UNKNOWN:
    default:
      PLAYER_ERROR("unknown ACTS version!");
      return(-1);
      break;
  }
  acts_args[i] = (char*)NULL;

  assert((unsigned int)i <= sizeof(acts_args) / sizeof(acts_args[0]));

  printf("\ninvoking ACTS with:\n\n    ");
  for(int j=0;acts_args[j];j++)
    printf("%s ", acts_args[j]);
  puts("\n");
  
  if(!(pid = fork()))
  {
    // make sure we don't get that "ACTS: Packet" bullshit on the console
    //int dummy_fd = open("/dev/null",O_RDWR);
    //dup2(dummy_fd,0);
    //dup2(dummy_fd,1);
    //dup2(dummy_fd,2);

    /* detach from controlling tty, so we don't get pesky SIGINTs and such */
    if(setpgid(0,0) == -1)
    {
      perror("Acts:Setup(): error while setpgrp()");
      exit(1);
    }

    // if no path to the binary was given, search the user's PATH
    if(!strlen(binarypath))
    {
      if(execvp(acts_bin_name,acts_args) == -1)
      {
        /* 
        * some error.  print it here.  it will really be detected
        * later when the parent tries to connect(2) to it
         */
        perror("Acts:Setup(): error while execvp()ing ACTS");
        exit(1);
      }
    }
    else
    {
      if(execv(binarypath,acts_args) == -1)
      {
        /* 
        * some error.  print it here.  it will really be detected
        * later when the parent tries to connect(2) to it
         */
        perror("Acts:Setup(): error while execv()ing ACTS");
        exit(1);
      }
    }
  }
  else
  {
    /* in parent */
    /* fill in addr structure */
    server.sin_family = PF_INET;
    /* 
     * this is okay to do, because gethostbyname(3) does no lookup if the 
     * 'host' * arg is already an IP addr
     */
    if((entp = gethostbyname(host)) == NULL)
    {
      fprintf(stderr, "Acts::Setup(): \"%s\" is unknown host; "
                      "can't connect to ACTS\n", host);
      /* try to kill ACTS just in case it's running */
      KillACTS();
      return(1);
    }

    memcpy(&server.sin_addr, entp->h_addr_list[0], entp->h_length);

    server.sin_port = htons(portnum);

    /* ok, we'll make this a bit smarter.  first, we wait a baseline amount
     * of time, then try to connect periodically for some predefined number
     * of times
     */
    usleep(ACTS_STARTUP_USEC);

    for(j = 0;j<ACTS_STARTUP_CONN_LIMIT;j++)
    {
      /* 
       * hook it up
       */
      
      // make a new socket, because connect() screws with the old one somehow
      if((sock = socket(PF_INET, SOCK_STREAM, 0)) < 0)
      {
        perror("Acts::Setup(): socket(2) failed");
        KillACTS();
        return(1);
      }
      if(connect(sock,(struct sockaddr*)&server, sizeof(server)) == 0)
        break;
      usleep(ACTS_STARTUP_INTERVAL_USEC);
    }
    if(j == ACTS_STARTUP_CONN_LIMIT)
    {
      perror("Acts::Setup(): connect(2) failed");
      KillACTS();
      return(1);
    }
    puts("Done.");

    /* now spawn reading thread */
    StartThread();

    return(0);
  }

  // shut up compiler!
  return(0);
}

int
Acts::Shutdown()
{
  /* if Setup() was never called, don't do anything */
  if(sock == -1)
    return(0);

  StopThread();

  sock = -1;
  puts("ACTS vision server has been shutdown");
  return(0);
}

void 
Acts::KillACTS()
{
  if(kill(pid,SIGKILL) == -1)
    perror("Acts::KillACTS(): some error while killing ACTS");
}

void
Acts::Main()
{
  int numread;
  int num_blobs;
  int i;
  
  // we'll convert the data into this structured buffer
  acts_data_t acts_data;
  
  // we'll write the data from this buffer
  player_blobfinder_data_t local_data;

  acts_blob_elt_t *src;
  player_blobfinder_blob_t *dst;

  // AH: this can't be very safe (buffer sizes)
  // first, we'll read into these two temporary buffers
  uint8_t acts_hdr_buf[sizeof(acts_data.header)];
  uint8_t acts_blob_buf[sizeof(acts_data.blobs)];

  char acts_request_packet = ACTS_REQUEST_PACKET;

  /* make sure we kill ACTS on exiting */
  pthread_cleanup_push(QuitACTS,this);

  /* loop and read */
  for(;;)
  {
    // clean our buffers
    memset(&acts_data,0,sizeof(acts_data));
    memset(&local_data,0,sizeof(local_data));
    
    // put in some stuff that doesnt change
    acts_data.width = htons(this->width);
    acts_data.height = htons(this->height);
    
    /* test if we are supposed to cancel */
    pthread_testcancel();

    /* request a packet from ACTS */
    if(write(sock,&acts_request_packet,sizeof(acts_request_packet)) == -1)
    {
      perror("RunVisionThread: write() failed sending ACTS_REQUEST_PACKET;"
             "exiting.");
      break;
    }

    /* get the header first */
    if((numread = read(sock,acts_hdr_buf,header_len)) == -1)
    {
      perror("RunVisionThread: read() failed for header: exiting");
      break;
    }
    else if(numread != header_len)
    {
      fprintf(stderr,"RunVisionThread: something went wrong\n"
              "              expected %d bytes of header, but only got %d\n", 
              header_len,numread);
      break;
    }

    /* convert the header, if necessary */
    if(acts_version == ACTS_VERSION_1_0)
    {
      for(i=0;i<ACTS_MAX_CHANNELS;i++)
      {
        // convert 2-byte ACTS 1.0 encoded entries to byte-swapped integers
        // in a structured array
        acts_data.header[i].index = 
          htons(acts_hdr_buf[header_elt_len*i]-1);
        acts_data.header[i].num = 
          htons(acts_hdr_buf[header_elt_len*i+1]-1);
      }
    }
    else
    {
      for(i=0;i<ACTS_MAX_CHANNELS;i++)
      {
        // convert 4-byte ACTS 1.2/2.0 encoded entries to byte-swapped integers
        // in a structured array
        acts_data.header[i].index = acts_hdr_buf[header_elt_len*i]-1;
        acts_data.header[i].index = 
          acts_data.header[i].index << 6;
        acts_data.header[i].index |= 
          acts_hdr_buf[header_elt_len*i+1]-1;
        acts_data.header[i].index = 
          htons(acts_data.header[i].index);

        acts_data.header[i].num = acts_hdr_buf[header_elt_len*i+2]-1;
        acts_data.header[i].num = 
          acts_data.header[i].num << 6;
        acts_data.header[i].num |= 
          acts_hdr_buf[header_elt_len*i+3]-1;
        acts_data.header[i].num = 
          htons(acts_data.header[i].num);
      }
    }

    /* sum up the data we expect */
    num_blobs=0;
    for(i=0;i<ACTS_MAX_CHANNELS;i++)
      num_blobs += ntohs(acts_data.header[i].num);

    /* read in the blob data */
    if((numread = read(sock,acts_blob_buf,num_blobs*blob_size)) == -1)
    {
      perror("RunVisionThread: read() failed on blob data; exiting.");
      break;
    }
    else if(numread != num_blobs*blob_size)
    {
      fprintf(stderr,"RunVisionThread: something went wrong\n"
              "              expected %d bytes of blob data, but only got %d\n", 
              num_blobs*blob_size,numread);
      break;
    }

    if(acts_version == ACTS_VERSION_1_0)
    {
      // convert 10-byte ACTS 1.0 blobs to new byte-swapped structured array
      for(i=0;i<num_blobs;i++)
      {
        int tmpptr = blob_size*i;

        // TODO: put a descriptive color in here (I'm not sure where
        // to get it from).
        acts_data.blobs[i].color = 0xFF0000;
        
        // get the 4-byte area first
        acts_data.blobs[i].area = 0;
        for(int j=0;j<4;j++)
        {
          acts_data.blobs[i].area = acts_data.blobs[i].area << 6;
          acts_data.blobs[i].area |= acts_blob_buf[tmpptr++] - 1;
        }

        // store the 1 byte values
        acts_data.blobs[i].x = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].y = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].left = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].right = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].top = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].bottom = acts_blob_buf[tmpptr++] - 1;
      }
    }
    else
    {
      // convert 16-byte ACTS 1.2/2.0 blobs to new byte-swapped structured array
      for(i=0;i<num_blobs;i++)
      {
        int tmpptr = blob_size*i;

        // Figure out the blob channel number
        int ch = 0;
        for (int j = 0; j < ACTS_MAX_CHANNELS; j++)
        {
          if (i >= acts_data.header[j].index &&
              i < acts_data.header[j].index + acts_data.header[j].num)
          {
            ch = j;
            break;
          }
        }

        // Put in a descriptive color.
        if (ch < (int) (sizeof(colors) / sizeof(colors[0])))
          acts_data.blobs[i].color = colors[ch];            
        else
          acts_data.blobs[i].color = 0xFF0000;
        
        // get the 4-byte area first
        acts_data.blobs[i].area = 0;
        for(int j=0;j<4;j++)
        {
          acts_data.blobs[i].area = acts_data.blobs[i].area << 6;
          acts_data.blobs[i].area |= acts_blob_buf[tmpptr++] - 1;
        }
        
        // Get the other 2 byte values
        acts_data.blobs[i].x = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].x = acts_data.blobs[i].x << 6;
        acts_data.blobs[i].x |= acts_blob_buf[tmpptr++] - 1;

        acts_data.blobs[i].y = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].y = acts_data.blobs[i].y << 6;
        acts_data.blobs[i].y |= acts_blob_buf[tmpptr++] - 1;

        acts_data.blobs[i].left = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].left = acts_data.blobs[i].left << 6;
        acts_data.blobs[i].left |= acts_blob_buf[tmpptr++] - 1;

        acts_data.blobs[i].right = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].right = acts_data.blobs[i].right << 6;
        acts_data.blobs[i].right |= acts_blob_buf[tmpptr++] - 1;

        acts_data.blobs[i].top = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].top = acts_data.blobs[i].top << 6;
        acts_data.blobs[i].top |= acts_blob_buf[tmpptr++] - 1;

        acts_data.blobs[i].bottom = acts_blob_buf[tmpptr++] - 1;
        acts_data.blobs[i].bottom = acts_data.blobs[i].bottom << 6;
        acts_data.blobs[i].bottom |= acts_blob_buf[tmpptr++] - 1;
      }
    }
    
    // Convert data to interface format
    local_data.width = acts_data.width;
    local_data.height = acts_data.height;
    local_data.blobs_count = num_blobs;
      
    for (i = 0; i < num_blobs; i++)
    {
      src = acts_data.blobs + i;
      dst = local_data.blobs + i;
      dst->id = 0;
      dst->color = src->color;
      dst->x = src->x;
      dst->y = src->y;
      dst->left = src->left;
      dst->right = src->right;
      dst->top = src->top;
      dst->bottom = src->bottom;
      dst->range = 0;
    }

    /* got the data. now fill it in */
    Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS, &local_data, sizeof(local_data) - sizeof(local_data.blobs) +
            (local_data.blobs_count) * sizeof(local_data.blobs[0]), NULL);
  }

  pthread_cleanup_pop(1);
  pthread_exit(NULL);
}

void 
QuitACTS(void* visiondevice)
{
  char acts_request_quit = ACTS_REQUEST_QUIT;
  Acts* vd = (Acts*)visiondevice;


  if((fcntl(vd->sock, F_SETFL, O_NONBLOCK) == -1) ||
     (write(vd->sock,&acts_request_quit,sizeof(acts_request_quit)) == -1))
  {
    perror("Acts::QuitACTS(): WARNING: either fcntl() or write() "
            "failed while sending QUIT command; killing ACTS by hand");
    vd->KillACTS();
  }
  vd->sock = -1;
}