camera1394.cc

机器人仿真软件

  if (!this->forceRaw &&
      dc1394_dma_setup_capture(this->handle, this->camera.node, channel,
                               this->format, this->mode, speed,
                               this->frameRate, this->num_dma_buffers, 1, 0, NULL,
                               &this->camera) == DC1394_SUCCESS)
#elif LIBDC1394_VERSION == 0200
  // Set camera to use DMA, improves performance.
  bool DMA_Success = true;
  if (!this->forceRaw)
  {
  	// first set parameters that are common between format 7 and other modes
  	if (DC1394_SUCCESS != dc1394_video_set_framerate(camera,frameRate))
	{
		PLAYER_WARN("1394 failed to set frameRate");
  		DMA_Success = false;
	}
  	if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera,speed))
	{
		PLAYER_WARN("1394 failed to set iso speed");
		DMA_Success = false;
	}
	if (DC1394_SUCCESS != dc1394_video_set_mode(camera,mode))
	{
		PLAYER_WARN("1394 failed to set mode");
		DMA_Success = false;
	}
  	
  	// now start capture
	if (DC1394_SUCCESS != dc1394_capture_setup_dma(camera, this->num_dma_buffers, DC1394_RING_BUFFER_LAST))
  		DMA_Success = false;
  }
  if (DMA_Success)
#else
  if (0)
#endif
  {    
    this->method = methodVideo;
  }
  else
  {
    PLAYER_WARN("DMA capture failed; falling back on RAW method");
          
    // Set camera to use RAW method (fallback)
#if LIBDC1394_VERSION == 0200
    if (0)
#else
    if (dc1394_setup_capture(this->handle, this->camera.node, channel,
                             this->format, this->mode, SPEED_400, this->frameRate,
                             &this->camera) == DC1394_SUCCESS)
#endif
    {
      this->method = methodRaw;
    }
    else
    {
      PLAYER_ERROR("unable to open camera in VIDE0 or RAW modes");
      this->SafeCleanup();
      return -1;
    }
  }

  // Start transmitting camera data
#if LIBDC1394_VERSION == 0200
  if (DC1394_SUCCESS != dc1394_video_set_transmission(this->camera, DC1394_ON))
#else
  if (DC1394_SUCCESS != dc1394_start_iso_transmission(this->handle, this->camera.node))
#endif
  {
    PLAYER_ERROR("unable to start camera");
    this->SafeCleanup();
    return -1;
  }

  // Start the driver thread.
  this->StartThread();
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int Camera1394::Shutdown()
{
  // Stop the driver thread.
  StopThread();

  // Stop transmitting camera data
#if LIBDC1394_VERSION == 0200
  if (DC1394_SUCCESS != dc1394_video_set_transmission(this->camera, DC1394_OFF))
#else
  if (dc1394_stop_iso_transmission(this->handle, this->camera.node) != DC1394_SUCCESS) 
#endif
    PLAYER_WARN("unable to stop camera");

  // Free resources
  this->SafeCleanup();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void Camera1394::Main() 
{
  char filename[255];
  int frameno;

  frameno = 0;
  //struct timeval now,old;
  while (true)
  {
    
    // Go to sleep for a while (this is a polling loop).
    // We shouldn't need to sleep if GrabFrame is blocking.    
    //nanosleep(&NSLEEP_TIME, NULL);

    // Test if we are supposed to cancel this thread.
    pthread_testcancel();

    // Process any pending requests.
    ProcessMessages();

    // Grab the next frame (blocking)
    this->GrabFrame();

    // Write data to server
    this->RefreshData();

    // this should go or be replaced
    // Save frames; must be done after writedata (which will byteswap)
    if (this->save)
    {
      //printf("click %d\n", frameno);
      snprintf(filename, sizeof(filename), "click-%04d.ppm", frameno++);
      this->SaveFrame(filename);
    }
    /*
      gettimeofday(&now,NULL);
      printf("dt = %lf\n",now.tv_sec-old.tv_sec+(now.tv_usec-old.tv_usec)*1.0e-6);
      old=now;
    */
 }
  printf("Camera1394::main() exited\n");
  
}


////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int Camera1394::ProcessMessage(MessageQueue* resp_queue,
                              player_msghdr * hdr,
                              void * data)
{
  assert(resp_queue);
  assert(hdr);
  assert(data);

  /* We currently don't support any messages, but if we do...
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                           PLAYER_FIDUCIAL_GET_GEOM, this->device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_data_t));
    ProcessOdom(hdr, *reinterpret_cast<player_position2d_data_t *> (data));
    return(0);

  }
  */

  return -1;
}

////////////////////////////////////////////////////////////////////////////////
// Store an image frame into the 'frame' buffer
int Camera1394::GrabFrame()
{
  uint f, c;
  int  i, j;
  unsigned char * ptr1, * ptr2, * dst;

  switch (this->method)
  {
  case methodRaw:
#if LIBDC1394_VERSION == 0200
    if (dc1394_capture(&this->camera, 1) != DC1394_SUCCESS)
#else
    if (dc1394_single_capture(this->handle, &this->camera) != DC1394_SUCCESS)
#endif
    {
      PLAYER_ERROR("Unable to capture frame");
      return -1;
    }
    break;
  case methodVideo:
#if LIBDC1394_VERSION == 0200
    if (dc1394_capture_dma(&this->camera,1,DC1394_VIDEO1394_WAIT) != DC1394_SUCCESS)
#else
    if (dc1394_dma_single_capture(&this->camera) != DC1394_SUCCESS)
#endif
    {
      PLAYER_ERROR("Unable to capture frame");
      return -1;
    }
    break;
  default:
    PLAYER_ERROR("Unknown grab method");
    return -1;
  }

  unsigned int frame_width;
  unsigned int frame_height;
  int * capture_buffer;
#if LIBDC1394_VERSION == 0200
  frame_width = dc1394_capture_get_width(camera);
  frame_height = dc1394_capture_get_height(camera);
  capture_buffer = (int *) dc1394_capture_get_dma_buffer(camera);
#else
  frame_width = this->camera.frame_width;
  frame_height = this->camera.frame_height;
  capture_buffer = this->camera.capture_buffer;
#endif

  if (frameSize == 0)
    frameSize = frame_width * frame_height;

  switch (this->mode)
  {
  case MODE_320x240_YUV422:
  case MODE_640x480_YUV422:
    this->data.bpp = 24;
    this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
    this->data.image_count = this->frameSize;
    this->data.width = frame_width;
    this->data.height = frame_height;
    assert(this->data.image_count <= sizeof(this->data.image));
    uyvy2rgb((unsigned char *)capture_buffer, this->data.image, (frame_width) * (frame_height));
    break;
  case MODE_1024x768_YUV422:
  case MODE_1280x960_YUV422:
    if (resized == NULL)
      resized = new unsigned char[1280 * 960 * 3];
    this->data.bpp = 24;
    this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
    this->data.image_count = this->frameSize;
    this->data.width = frame_width / 2;
    this->data.height = frame_height / 2;
    assert(this->data.image_count <= sizeof(this->data.image));
    uyvy2rgb((unsigned char *)capture_buffer, this->resized, (frame_width) * (frame_height));
    ptr1 = this->resized;
    ptr2 = this->data.image;
    for (f = 0; f < (this->data.height); f++)
    {
      for (c = 0; c < (this->data.width); c++)
      {
	ptr2[0] = ptr1[0];
	ptr2[1] = ptr1[1];
	ptr2[2] = ptr1[2];
	ptr1 += (3 * 2);
	ptr2 += 3;
      }
      ptr1 += ((frame_width) * 3);
    }
    break;
  case MODE_800x600_YUV422:
    if (resized == NULL)
      resized = new unsigned char[1280 * 960 * 3];
    this->data.bpp = 24;
    this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
    this->data.image_count = this->frameSize;
    this->data.width = 600;
    this->data.height = 450;
    assert(this->data.image_count <= sizeof(this->data.image));
    uyvy2rgb((unsigned char *)capture_buffer, this->resized, (frame_width) * (frame_height));
    ptr1 = this->resized;
    ptr2 = this->data.image;
    i = 3; j = 3;
    for (f = 0; f < (this->data.height); f++)
    {
      for (c = 0; c < (this->data.width); c++)
      {
	ptr2[0] = ptr1[0];
	ptr2[1] = ptr1[1];
	ptr2[2] = ptr1[2];
	j--;
	if (!j) ptr1 += (3 * 2), j = 3;
	else ptr1 += 3;
	ptr2 += 3;
      }
      i--;
      if (!i) ptr1 += ((frame_width) * 3), i = 3;
    }
    break;  
  case MODE_640x480_RGB:
    this->data.bpp = 24;
    this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
    this->data.image_count = this->frameSize;
    this->data.width = frame_width;
    this->data.height = frame_height;
    assert(this->data.image_count <= sizeof(this->data.image));
    memcpy(this->data.image, (unsigned char *)capture_buffer, this->data.image_count);
    break;
  case MODE_640x480_MONO:
  case MODE_800x600_MONO:
  case MODE_1024x768_MONO:
  case MODE_1280x960_MONO:
  case MODE_FORMAT7_0:
    if (!DoBayerConversion)
    {
      this->data.bpp = 8;
      this->data.format = PLAYER_CAMERA_FORMAT_MONO8;
      this->data.image_count = this->frameSize;
      this->data.width = frame_width;
      this->data.height = frame_height;
      assert(this->data.image_count <= sizeof(this->data.image));
      memcpy(this->data.image, (unsigned char *)capture_buffer, this->data.image_count);
    } 
    else
    {
      this->data.bpp = 24;
      this->data.format = PLAYER_CAMERA_FORMAT_RGB888;
      if ((frame_width) > PLAYER_CAMERA_IMAGE_WIDTH)
      {
        if (resized == NULL)
          resized = new unsigned char[this->frameSize * 3];
        dst = this->resized;
      }
      else 
      {
        dst = this->data.image;
      }
      switch (this->BayerMethod)
      {
      case BAYER_DECODING_DOWNSAMPLE:
        // quarter of the image but 3 bytes per pixel
	this->data.image_count = this->frameSize/4*3;
	assert(this->data.image_count <= sizeof(this->data.image));
	BayerDownsample((unsigned char *)capture_buffer, this->data.image,
					frame_width/2, frame_height/2,
					(bayer_pattern_t)this->BayerPattern);
	break;
      case BAYER_DECODING_NEAREST:
        if ((frame_width) > PLAYER_CAMERA_IMAGE_WIDTH) this->data.image_count = this->frameSize/4*3;
	else this->data.image_count = this->frameSize * 3;
	assert(this->data.image_count <= sizeof(this->data.image));
	BayerNearestNeighbor((unsigned char *)capture_buffer, dst,
					frame_width, frame_height,
					(bayer_pattern_t)this->BayerPattern);
	break;
      case BAYER_DECODING_EDGE_SENSE:
        if ((frame_width) > PLAYER_CAMERA_IMAGE_WIDTH) this->data.image_count = this->frameSize/4*3;
        else this->data.image_count = this->frameSize * 3;
	assert(this->data.image_count <= sizeof(this->data.image));
	BayerEdgeSense((unsigned char *)capture_buffer, dst,
					frame_width, frame_height,
					(bayer_pattern_t)this->BayerPattern);
	break;
      default:
        PLAYER_ERROR("camera1394: Unknown Bayer Method");
	return -1;
      }
      if (this->BayerMethod != BAYER_DECODING_DOWNSAMPLE)
      {
        if ((frame_width) > PLAYER_CAMERA_IMAGE_WIDTH)
	{
	  this->data.width = frame_width/2;
	  this->data.height = frame_height/2;
          ptr1 = this->resized;
          ptr2 = this->data.image;
          for (f = 0; f < (this->data.height); f++)
          {
            for (c = 0; c < (this->data.width); c++)
            {
	      ptr2[0] = ptr1[0];
	      ptr2[1] = ptr1[1];
	      ptr2[2] = ptr1[2];
	      ptr1 += (3 * 2);
	      ptr2 += 3;
            }
            ptr1 += ((frame_width) * 3);
          }
	} else
	{
	  this->data.width = frame_width;
	  this->data.height = frame_height;
	}
      } else
      {    //image is half the size of grabbed frame
	this->data.width = frame_width/2;
	this->data.height = frame_height/2;
      }
    }
    break;
  default:
    PLAYER_ERROR("camera1394: Unknown mode");
    return -1;
  }
#if LIBDC1394_VERSION == 0200
  if (this->method == methodVideo) dc1394_capture_dma_done_with_buffer(this->camera);
#else
  if (this->method == methodVideo) dc1394_dma_done_with_buffer(&this->camera);
#endif

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void Camera1394::RefreshData()
{
  size_t size;

  // Work out the data size
  size = sizeof(this->data) - sizeof(this->data.image) + this->data.image_count;

  // now we just do the byte-swapping
  this->data.width = this->data.width;
  this->data.height = this->data.height;

  this->data.compression = PLAYER_CAMERA_COMPRESS_RAW;
  this->data.image_count = this->data.image_count;

  /* We should do this to be efficient */
  Publish(this->device_addr, NULL,
          PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
          reinterpret_cast<void*>(&this->data), size, NULL);  
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Save a frame to disk
int Camera1394::SaveFrame(const char *filename)
{
  FILE *fp = fopen(filename, "wb");

  if (fp == NULL)
  {
    PLAYER_ERROR("Couldn't create image file");
    return -1;
  }

  switch (this->data.format)
  {
    case PLAYER_CAMERA_FORMAT_MONO8:
      fprintf(fp,"P5\n%u %u\n255\n", this->data.width, this->data.height);
      fwrite((unsigned char*)this->data.image, 1, this->data.image_count, fp);
      break;
    case PLAYER_CAMERA_FORMAT_RGB888:
      fprintf(fp,"P6\n%u %u\n255\n", this->data.width, this->data.height);
      fwrite((unsigned char*)this->data.image, 1, this->data.image_count, fp);
      break;
    default:
      break;
  }

  fclose(fp);

  return 0;
}