acts.cc

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

        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 transform the data into this structured buffer
  player_blobfinder_data_t local_data;

  // first, we'll read into these two temporary buffers
  uint8_t acts_hdr_buf[sizeof(local_data.header)];
  uint8_t acts_blob_buf[sizeof(local_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
    bzero(&local_data,sizeof(local_data));
    
    // put in some stuff that doesnt change
    local_data.width = htons(this->width);
    local_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<PLAYER_BLOBFINDER_MAX_CHANNELS;i++)
      {
        // convert 2-byte ACTS 1.0 encoded entries to byte-swapped integers
        // in a structured array
        local_data.header[i].index = 
                htons(acts_hdr_buf[header_elt_len*i]-1);
        local_data.header[i].num = 
                htons(acts_hdr_buf[header_elt_len*i+1]-1);
      }
    }
    else
    {
      for(i=0;i<PLAYER_BLOBFINDER_MAX_CHANNELS;i++)
      {
        // convert 4-byte ACTS 1.2/2.0 encoded entries to byte-swapped integers
        // in a structured array
        local_data.header[i].index = acts_hdr_buf[header_elt_len*i]-1;
        local_data.header[i].index = 
                local_data.header[i].index << 6;
        local_data.header[i].index |= 
                acts_hdr_buf[header_elt_len*i+1]-1;
        local_data.header[i].index = 
                htons(local_data.header[i].index);

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

    /* sum up the data we expect */
    num_blobs=0;
    for(i=0;i<PLAYER_BLOBFINDER_MAX_CHANNELS;i++)
      num_blobs += ntohs(local_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).
        local_data.blobs[i].color = htonl(0xFF0000);
        
        // stage puts the range in here to simulate stereo vision. we
        // can't do that (yet?) so set the range to zero - rtv
        local_data.blobs[i].range = 0;

        // get the 4-byte area first
        local_data.blobs[i].area = 0;
        for(int j=0;j<4;j++)
        {
          local_data.blobs[i].area = local_data.blobs[i].area << 6;
          local_data.blobs[i].area |= acts_blob_buf[tmpptr++] - 1;
        }
        local_data.blobs[i].area = htonl(local_data.blobs[i].area);

        // convert the other 6 one-byte entries to byte-swapped shorts
        local_data.blobs[i].x = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].x = htons(local_data.blobs[i].x);

        local_data.blobs[i].y = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].y = htons(local_data.blobs[i].y);

        local_data.blobs[i].left = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].left = htons(local_data.blobs[i].left);

        local_data.blobs[i].right = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].right = htons(local_data.blobs[i].right);

        local_data.blobs[i].top = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].top = htons(local_data.blobs[i].top);

        local_data.blobs[i].bottom = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].bottom = htons(local_data.blobs[i].bottom);
      }
    }
    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 < PLAYER_BLOBFINDER_MAX_CHANNELS; j++)
        {
          if (i >= ntohs(local_data.header[j].index) &&
              i < ntohs(local_data.header[j].index) + ntohs(local_data.header[j].num))
          {
            ch = j;
            break;
          }
        }

        // Put in a descriptive color.
        if (ch < (int) (sizeof(colors) / sizeof(colors[0])))
          local_data.blobs[i].color = htonl(colors[ch]);            
        else
          local_data.blobs[i].color = htonl(0xFF0000);

        // stage puts the range in here to simulate stereo vision. we
        // can't do that (yet?) so set the range to zero - rtv
        local_data.blobs[i].range = 0;

        // get the 4-byte area first
        local_data.blobs[i].area = 0;
        for(int j=0;j<4;j++)
        {
          local_data.blobs[i].area = local_data.blobs[i].area << 6;
          local_data.blobs[i].area |= acts_blob_buf[tmpptr++] - 1;
        }
        local_data.blobs[i].area = htonl(local_data.blobs[i].area);
        
        // convert the other 6 two-byte entries to byte-swapped shorts
        local_data.blobs[i].x = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].x = local_data.blobs[i].x << 6;
        local_data.blobs[i].x |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].x = htons(local_data.blobs[i].x);

        local_data.blobs[i].y = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].y = local_data.blobs[i].y << 6;
        local_data.blobs[i].y |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].y = htons(local_data.blobs[i].y);

        local_data.blobs[i].left = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].left = local_data.blobs[i].left << 6;
        local_data.blobs[i].left |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].left = htons(local_data.blobs[i].left);

        local_data.blobs[i].right = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].right = local_data.blobs[i].right << 6;
        local_data.blobs[i].right |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].right = htons(local_data.blobs[i].right);

        local_data.blobs[i].top = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].top = local_data.blobs[i].top << 6;
        local_data.blobs[i].top |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].top = htons(local_data.blobs[i].top);

        local_data.blobs[i].bottom = acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].bottom = local_data.blobs[i].bottom << 6;
        local_data.blobs[i].bottom |= acts_blob_buf[tmpptr++] - 1;
        local_data.blobs[i].bottom = htons(local_data.blobs[i].bottom);
      }
    }
    
    /* test if we are supposed to cancel */
    pthread_testcancel();

    /* got the data. now fill it in */
    PutData((unsigned char*)&local_data, 
                (PLAYER_BLOBFINDER_HEADER_SIZE + 
                 num_blobs*PLAYER_BLOBFINDER_BLOB_SIZE),0,0);
  }

  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;
}