Exemple #1
1
void UvcVideo::InitDevice(int vid, int pid, const char* sn, int device_id, int width, int height, int fps)
{
    uvc_error_t find_err = FindDevice(ctx_, &dev_, vid, pid, sn, device_id );
    if (find_err != UVC_SUCCESS) {
        uvc_perror(find_err, "uvc_find_device");
        throw VideoException("Unable to open UVC Device");
    }
    if(!dev_) {
        throw VideoException("Unable to open UVC Device - no pointer returned.");
    }
    
    uvc_error_t open_err = uvc_open(dev_, &devh_);
    if (open_err != UVC_SUCCESS) {
        uvc_perror(open_err, "uvc_open");
        uvc_unref_device(dev_);
        throw VideoException("Unable to open device");
    }

    //uvc_print_diag(devh_, stderr);
    
    uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
                devh_, &ctrl_,
                UVC_FRAME_FORMAT_ANY,
                width, height,
                fps);

    //uvc_print_stream_ctrl(&ctrl_, stderr);
            
    if (mode_err != UVC_SUCCESS) {
        uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
        uvc_close(devh_);
        uvc_unref_device(dev_);
        throw VideoException("Unable to set device mode.");
    }

    uvc_error_t strm_err = uvc_stream_open_ctrl(devh_, &strm_, &ctrl_);
    if(strm_err != UVC_SUCCESS) {
        uvc_perror(strm_err, "uvc_stream_open_ctrl");
        uvc_close(devh_);
        uvc_unref_device(dev_);
        throw VideoException("Unable to open device stream.");
    }

    // Default to greyscale.
    PixelFormat pfmt = PixelFormatFromString("GRAY8");

    const uvc_format_desc_t* uvc_fmt = uvc_get_format_descs(devh_);
    while( uvc_fmt->bFormatIndex != ctrl_.bFormatIndex && uvc_fmt ) {
        uvc_fmt = uvc_fmt->next;
    }

    if(uvc_fmt) {
        // TODO: Use uvc_fmt->fourccFormat
        if( uvc_fmt->bBitsPerPixel == 16 ) {
            pfmt = PixelFormatFromString("GRAY16LE");
        }
    }
    
    const StreamInfo stream_info(pfmt, width, height, (width*pfmt.bpp)/8, 0);
    streams.push_back(stream_info);
}
Exemple #2
1
void RealSenseDriver::Start(int vid, int pid, char* sn)
{
  //The RealSense camera is capable of delivering separate streams for rgb and depth at separate resolutions
  //For now, use 640x480@30fps for each one
  //There are separate streams defined for depth using non-standard UVC GUIDs - use the YUYV-advertised GUID, which is really just 
  //gray16 data


    width_ = 640;
    height_ = 480;
    fps_ = 30;
    
    if(ctx_) {
        Stop();
    }
    
    uvc_init(&ctx_, NULL);
    if(!ctx_) {
        throw DeviceException("Unable to open UVC library context");
    }

    uvc_error_t find_err;
    if (sn)
      find_err = uvc_find_device(ctx_, &dev_, vid, pid, sn );
    else
      find_err = uvc_find_device(ctx_, &dev_, vid, pid, NULL);

    if (find_err != UVC_SUCCESS) {
        uvc_perror(find_err, "uvc_find_device");
        throw DeviceException("Unable to open RealSense device");
    }
    if(!dev_) {
        throw DeviceException("Unable to open RealSense Device - no pointer returned.");
    }
    
    uvc_error_t open_err = uvc_open2(dev_, &devh_rgb, 0); //Camera 0 is RGB
    if (open_err != UVC_SUCCESS) {
        uvc_perror(open_err, "uvc_open");
        uvc_unref_device(dev_);
        throw DeviceException("Unable to open RealSense RGB device");
    }

    open_err = uvc_open2(dev_, &devh_d, 1); //Camera 1 is depth
    if (open_err != UVC_SUCCESS) {
        uvc_perror(open_err, "uvc_open");
        uvc_unref_device(dev_);
        throw DeviceException("Unable to open RealSense Depth device");
    }
    
  
//    uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
    

    
    uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
                devh_rgb, &ctrl_rgb,
                UVC_COLOR_FORMAT_YUYV,
                width_, height_,
                60);
    
    pbtype_rgb = pb::PB_UNSIGNED_BYTE;
    pbformat_rgb = pb::PB_RGB;
        
    if (mode_err != UVC_SUCCESS) {
        uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
        uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to get RGB device mode.");
    }

    //Use the same frame parameters for both RGB and depth (for now)
    if (useIR)
      {
	//Depth as mono8
	mode_err = uvc_get_stream_ctrl_format_size(
						   devh_d, &ctrl_d,
						   UVC_FRAME_FORMAT_INVI,
						   width_, height_,
						   fps_);
	
	pbtype_d = pb::PB_UNSIGNED_BYTE;
      }
    else
      {
	//Grayscale depth as yuyv, mono16
	mode_err = uvc_get_stream_ctrl_format_size(
						   devh_d, &ctrl_d,
						   //UVC_FRAME_FORMAT_INVI,
						   UVC_FRAME_FORMAT_YUYV,
						   width_, height_,
						   fps_);
    
	pbtype_d = pb::PB_UNSIGNED_SHORT;
      }
    
    pbformat_d = pb::PB_LUMINANCE;
        
    if (mode_err != UVC_SUCCESS) {
        uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
        uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to get depth device mode.");
    }

    uvc_error_t stream_ctl_err = uvc_stream_open_ctrl(devh_rgb, &streamh_rgb, &ctrl_rgb);
   
    if (stream_ctl_err != UVC_SUCCESS) {
        uvc_perror(stream_ctl_err, "uvc_stream_open_ctrl");
        uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to open stream on RGB.");
    }

    stream_ctl_err = uvc_stream_open_ctrl(devh_d, &streamh_d, &ctrl_d);
   
    if (stream_ctl_err != UVC_SUCCESS) {
        uvc_perror(stream_ctl_err, "uvc_stream_open_ctrl");
        uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to open stream on depth.");
    }

    uvc_error_t stream_start_err = uvc_stream_start(streamh_rgb, NULL, this, 0);
    
    if (stream_start_err != UVC_SUCCESS) {
        uvc_perror(stream_start_err, "uvc_stream_start");
	uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to start streaming on rgb.");
    }

    stream_start_err = uvc_stream_start(streamh_d, NULL, this, 0);

    if (stream_start_err != UVC_SUCCESS) {
        uvc_perror(stream_start_err, "uvc_stream_start");
	uvc_close(devh_rgb);
	uvc_close(devh_d);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to start streaming on depth.");
    }
    
    if (frame_rgb)
        uvc_free_frame(frame_rgb);
     if (frame_d)
        uvc_free_frame(frame_d);
    
    
    frame_rgb = uvc_allocate_frame(ctrl_rgb.dwMaxVideoFrameSize);
    if(!frame_rgb) {
        throw DeviceException("Unable to allocate RGB frame.");
    }

    frame_d = uvc_allocate_frame(ctrl_d.dwMaxVideoFrameSize);
    if(!frame_d) {
        throw DeviceException("Unable to allocate depth frame.");
    }

    if (useSync)
      {
	std::cout << "RealSense: Using SCR to sync" << std::endl;
      }
}
            void set_power_state(power_state state) override {
                uvc_error_t res;
                uvc_format_t *formats;
                /* if power became on and it was originally off. open the uvc device. */
                if (state == D0 && _state == D3) {
                    res = uvc_find_device(_ctx, &_device, _info.vid, _info.pid, NULL);

                    if (res < 0) {
                        throw linux_backend_exception(
                                "Could not find the device.");
                    }
                    res = uvc_open2(_device, &_device_handle, _interface);

                    if (res < 0) {
                        uvc_unref_device(_device);
                        _device = NULL;
                        throw linux_backend_exception(
                                "Could not open device.");
                    }
                }
                else {
                    // we have been asked to close the device.
                    uvc_unref_device(_device);
                    uvc_stop_streaming(_device_handle);
                    _profiles.clear();
                    uvc_close(_device_handle);
                    _device = NULL;
                    _device_handle = NULL;


                }

                _state = state;
            }
            /* request to set up a streaming profile and its calback */
            void probe_and_commit(stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override
            {
                uvc_error_t res;
                uvc_stream_ctrl_t ctrl;

                // request all formats for all pins in the device.
                res = uvc_get_stream_ctrl_format_size_all(
                        _device_handle, &ctrl,
                        profile.format,
                        profile.width,
                        profile.height,
                        profile.fps);

                if (res < 0) {
                    uvc_close(_device_handle);
                    uvc_unref_device(_device);
                    throw linux_backend_exception(
                            "Could not get stream format.");
                }

                // add to the vector of profiles.
                _profiles.push_back(profile);
                _callbacks.push_back(callback);
                _stream_ctrls.push_back(ctrl);
            }
Exemple #5
0
void RealSenseDriver::Stop()
{
  std::cout << "Stopping RealSense driver" << std::endl;

  /*
    if(streamh_rgb) {
        uvc_stream_close(streamh_rgb);
    }

    if(streamh_d) {
        uvc_stream_close(streamh_d);
    }
  */
    std::cout << "Closed streams" << std::endl;

    if (frame_rgb) {
        uvc_free_frame(frame_rgb);
        frame_rgb = 0;
    }    

    if (frame_d) {
        uvc_free_frame(frame_d);
        frame_d = 0;
    }
  
    std::cout << "Released frames" << std::endl;

    /* Release the device descriptor, shutdown UVC */
    uvc_close(devh_rgb);
    uvc_close(devh_d);

    std::cout << "Closed devices" << std::endl;

    
    uvc_unref_device(dev_);
    std::cout << "Unreffed device" << std::endl;


    uvc_exit(ctx_);
    dev_ = 0;

    std::cout << "Stop of RealSense driver complete" << std::endl;
 
}
void CameraDriver::CloseCamera() {
  assert(state_ == kRunning);

  uvc_close(devh_);
  devh_ = NULL;

  uvc_unref_device(dev_);
  dev_ = NULL;

  state_ = kStopped;
}
Exemple #7
0
            ~device()
            {
                for(auto interface_number : claimed_interfaces)
                {
                    int status = libusb_release_interface(get_subdevice(0).handle->usb_devh, interface_number);
                    if(status < 0) LOG_ERROR("libusb_release_interface(...) returned " << libusb_error_name(status));
                }

                for(auto & sub : subdevices) if(sub.handle) uvc_close(sub.handle);
                if(claimed_interfaces.size()) if(uvcdevice) uvc_unref_device(uvcdevice);
            }
Exemple #8
0
int UVCCamera::release() {
	ENTER();
	stopPreview();
	if (LIKELY(mDeviceHandle)) {
		SAFE_DELETE(mPreview);
		uvc_close(mDeviceHandle);
		mDeviceHandle = NULL;
	}
	if (LIKELY(mDevice)) {
		uvc_unref_device(mDevice);
		mDevice = NULL;
	}
	clearCameraParams();
	mCtrlSupports = mPUSupports = 0;
	if (LIKELY(mFd)) {
		close(mFd);
		mFd = 0;
	}
	RETURN(0, int);
}
Exemple #9
0
void UvcVideo::Start()
{
    uvc_error_t stream_err = uvc_stream_start(strm_, NULL, this, 0);
    
    if (stream_err != UVC_SUCCESS) {
        uvc_perror(stream_err, "uvc_stream_start");
        uvc_close(devh_);
        uvc_unref_device(dev_);
        throw VideoException("Unable to start streaming.");
    }
    
    if (frame_) {
        uvc_free_frame(frame_);
    }
    
    size_bytes = ctrl_.dwMaxVideoFrameSize;
    frame_ = uvc_allocate_frame(size_bytes);
    if(!frame_) {
        throw VideoException("Unable to allocate frame.");
    }
}
void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
  assert(state_ == kStopped);

  int vendor_id = strtol(new_config.vendor.c_str(), NULL, 0);
  int product_id = strtol(new_config.product.c_str(), NULL, 0);

  ROS_INFO("Opening camera with vendor=0x%x, product=0x%x, serial=\"%s\", index=%d",
           vendor_id, product_id, new_config.serial.c_str(), new_config.index);

  uvc_device_t **devs;

  // Implement missing index select behavior
  // https://github.com/ros-drivers/libuvc_ros/commit/4f30e9a0
#if libuvc_VERSION     > 00005 /* version > 0.0.5 */
  uvc_error_t find_err = uvc_find_devices(
    ctx_, &devs,
    vendor_id,
    product_id,
    new_config.serial.empty() ? NULL : new_config.serial.c_str());

  if (find_err != UVC_SUCCESS) {
    uvc_perror(find_err, "uvc_find_device");
    return;
  }

  // select device by index
  dev_ = NULL;
  int dev_idx = 0;
  while (devs[dev_idx] != NULL) {
    if(dev_idx == new_config.index) {
      dev_ = devs[dev_idx];
    }
    else {
      uvc_unref_device(devs[dev_idx]);
    }

    dev_idx++;
  }

  if(dev_ == NULL) {
    ROS_ERROR("Unable to find device at index %d", new_config.index);
    return;
  }
#else
  uvc_error_t find_err = uvc_find_device(
    ctx_, &dev_,
    vendor_id,
    product_id,
    new_config.serial.empty() ? NULL : new_config.serial.c_str());

  if (find_err != UVC_SUCCESS) {
    uvc_perror(find_err, "uvc_find_device");
    return;
  }

#endif
  uvc_error_t open_err = uvc_open(dev_, &devh_);

  if (open_err != UVC_SUCCESS) {
    switch (open_err) {
    case UVC_ERROR_ACCESS:
#ifdef __linux__
      ROS_ERROR("Permission denied opening /dev/bus/usb/%03d/%03d",
                uvc_get_bus_number(dev_), uvc_get_device_address(dev_));
#else
      ROS_ERROR("Permission denied opening device %d on bus %d",
                uvc_get_device_address(dev_), uvc_get_bus_number(dev_));
#endif
      break;
    default:
#ifdef __linux__
      ROS_ERROR("Can't open /dev/bus/usb/%03d/%03d: %s (%d)",
                uvc_get_bus_number(dev_), uvc_get_device_address(dev_),
                uvc_strerror(open_err), open_err);
#else
      ROS_ERROR("Can't open device %d on bus %d: %s (%d)",
                uvc_get_device_address(dev_), uvc_get_bus_number(dev_),
                uvc_strerror(open_err), open_err);
#endif
      break;
    }

    uvc_unref_device(dev_);
    return;
  }

  uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);

  uvc_stream_ctrl_t ctrl;
  uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
    devh_, &ctrl,
    GetVideoMode(new_config.video_mode),
    new_config.width, new_config.height,
    new_config.frame_rate);

  if (mode_err != UVC_SUCCESS) {
    uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
    uvc_close(devh_);
    uvc_unref_device(dev_);
    ROS_ERROR("check video_mode/width/height/frame_rate are available");
    uvc_print_diag(devh_, NULL);
    return;
  }

  uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this, 0);

  if (stream_err != UVC_SUCCESS) {
    uvc_perror(stream_err, "uvc_start_streaming");
    uvc_close(devh_);
    uvc_unref_device(dev_);
    return;
  }

  if (rgb_frame_)
    uvc_free_frame(rgb_frame_);

  rgb_frame_ = uvc_allocate_frame(new_config.width * new_config.height * 3);
  assert(rgb_frame_);

  state_ = kRunning;
}
int main(int argc, char **argv) {
  uvc_context_t *ctx;
  uvc_device_t *dev;
  uvc_device_handle_t *devh;
  uvc_stream_ctrl_t ctrl;
  uvc_error_t res;
  uvc_device_t **list;

  /* Initialize a UVC service context. Libuvc will set up its own libusb
   * context. Replace NULL with a libusb_context pointer to run libuvc
   * from an existing libusb context. */
  res = uvc_init(&ctx, NULL);


  if (res < 0) {
    uvc_perror(res, "uvc_init");
    return res;
  }
  puts("UVC initialized");
  res = uvc_get_device_list(ctx, &list);
  if (res < 0) {
    uvc_perror(res, "uvc_get_device_list");
    return res;
  }

/*  for(int i = 0; i < 1; i++) {
	 res = uvc_open(list[i], &devh);
	 uvc_print_diag(devh, stderr);
	 uvc_close(devh);
  }
  uchar x;
  uvc_free_device_list(list, x);
*/
  /* Locates the first attached UVC device, stores in dev */
  res = uvc_find_device(
      ctx, &dev,
      0, 0, NULL); /* filter devices: vendor_id, product_id, "serial_num" */

  if (res < 0) {
    uvc_perror(res, "uvc_find_device"); /* no devices found */
  } else {
    puts("Device found");

    /* Try to open the device: requires exclusive access */
    res = uvc_open(dev, &devh);

    if (res < 0) {
      uvc_perror(res, "uvc_open"); /* unable to open device */
      printf("Unable to open device");
    } else {
      puts("Device opened");

      /* Print out a message containing all the information that libuvc
       * knows about the device */
      uvc_print_diag(devh, stderr);

      /* Try to negotiate a 640x480 30 fps YUYV stream profile */
      res = uvc_get_stream_ctrl_format_size(
          devh, &ctrl, /* result stored in ctrl */
          UVC_FRAME_FORMAT_YUYV, /* YUV 422, aka YUV 4:2:2. try _COMPRESSED */
          640, 480, 30 /* width, height, fps */
      );

      /* Print out the result */
      uvc_print_stream_ctrl(&ctrl, stderr);

      if (res < 0) {
        uvc_perror(res, "get_mode"); /* device doesn't provide a matching stream */
      } else {
        /* Start the video stream. The library will call user function cb:
         *   cb(frame, (void*) 12345)
         */
        res = uvc_start_streaming(devh, &ctrl, cb, (void*)12345, 0);

        if (res < 0) {
          uvc_perror(res, "start_streaming"); /* unable to start stream */
        } else {
          puts("Streaming...");

          uvc_set_ae_mode(devh, 0); /* e.g., turn on auto exposure */

          sleep(10); /* stream for 10 seconds */

          /* End the stream. Blocks until last callback is serviced */
          uvc_stop_streaming(devh);
          puts("Done streaming.");
        }
      }

      /* Release our handle on the device */
      uvc_close(devh);
      puts("Device closed");
    }

    /* Release the device descriptor */
    uvc_unref_device(dev);
  }

  /* Close the UVC context. This closes and cleans up any existing device handles,
   * and it closes the libusb context if one was not provided. */
  uvc_exit(ctx);
  puts("UVC exited");

  return 0;
}
Exemple #12
0
void UvcDriver::Start(int vid, int pid, char* sn)
{
    width_ = 640;
    height_ = 480;
    fps_ = 30;
    
    if(ctx_) {
        Stop();
    }
    
    uvc_init(&ctx_, NULL);
    if(!ctx_) {
        throw DeviceException("Unable to open UVC Context");
    }
    
    uvc_error_t find_err = uvc_find_device(ctx_, &dev_, vid, pid, sn );
    if (find_err != UVC_SUCCESS) {
        uvc_perror(find_err, "uvc_find_device");
        throw DeviceException("Unable to open UVC Device");
    }
    if(!dev_) {
        throw DeviceException("Unable to open UVC Device - no pointer returned.");
    }
    
    uvc_error_t open_err = uvc_open(dev_, &devh_);
    if (open_err != UVC_SUCCESS) {
        uvc_perror(open_err, "uvc_open");
        uvc_unref_device(dev_);
        throw DeviceException("Unable to open device");
    }



    
//    uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
    
    uvc_stream_ctrl_t ctrl;
    uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
                devh_, &ctrl,
                UVC_COLOR_FORMAT_YUYV,
                width_, height_,
                fps_);
    
    pbtype = hal::PB_UNSIGNED_BYTE;
    pbformat = hal::PB_LUMINANCE;
        
    if (mode_err != UVC_SUCCESS) {
        uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
        uvc_close(devh_);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to device mode.");
    }
    
//    uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &UvcDriver::ImageCallbackAdapter, this);
    //uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, NULL, this);
    uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, NULL, this, 0);
    
    if (stream_err != UVC_SUCCESS) {
        uvc_perror(stream_err, "uvc_start_iso_streaming");
        uvc_close(devh_);
        uvc_unref_device(dev_);
        throw DeviceException("Unable to start iso streaming.");
    }
    
    if (frame_)
        uvc_free_frame(frame_);
    
    frame_ = uvc_allocate_frame(ctrl.dwMaxVideoFrameSize);
    if(!frame_) {
        throw DeviceException("Unable to allocate frame.");
    }
}