libuvc_camera.cpp 2.6 KB
Newer Older
Ken Tossell's avatar
Ken Tossell committed
1
2
3
4
5
/** @file test_ros_ctrls.cpp Example/test usage of libuvc */
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include "libuvc/libuvc.h"

6
7
using std::string;

Ken Tossell's avatar
Ken Tossell committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
ros::Publisher pub;

void cb(uvc_frame_t *frame) {
  static uvc_frame_t *rgb_frame = NULL;
  uvc_error_t uvc_ret;

  if (!rgb_frame)
    rgb_frame = uvc_allocate_frame(frame->width * frame->height * 3);

  uvc_ret = uvc_any2rgb(frame, rgb_frame);

  if (uvc_ret) {
    uvc_perror(uvc_ret, "Couldn't convert frame to RGB");
    return;
  }

  sensor_msgs::Image image;
  image.width = rgb_frame->width;
  image.height = rgb_frame->height;
  image.encoding = "rgb8";
  image.step = image.width * 3;
  image.data.resize(image.step * image.height);
  memcpy(&(image.data[0]), rgb_frame->data, rgb_frame->data_bytes);

  pub.publish(image);
}

int main (int argc, char **argv) {
  ros::init(argc, argv, "libuvc_camera");
  ros::NodeHandle nh;
38
  ros::NodeHandle priv_nh("~");
Ken Tossell's avatar
Ken Tossell committed
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

  pub = nh.advertise<sensor_msgs::Image>("image_raw", 1);

  uvc_context_t *ctx;
  uvc_error_t res;
  uvc_device_t *dev;
  uvc_device_handle_t *devh;
  uvc_stream_ctrl_t ctrl;

  res = uvc_init(&ctx, NULL);

  if (res < 0) {
    uvc_perror(res, "uvc_init");
    return res;
  }

  puts("UVC initialized");
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71

  int vendor_id;
  string vendor_id_str;
  priv_nh.getParam("vendor", vendor_id_str);
  vendor_id = strtol(vendor_id_str.c_str(), NULL, 0);

  int product_id;
  string product_id_str;
  priv_nh.getParam("product", product_id_str);
  product_id = strtol(product_id_str.c_str(), NULL, 0);

  string serial_num;
  priv_nh.getParam("serial", serial_num);

  printf("pid: %d\n", product_id);
  printf("vid: %d\n", vendor_id);
Ken Tossell's avatar
Ken Tossell committed
72
73
74
  
  res = uvc_find_device(
    ctx, &dev,
75
76
77
    vendor_id,
    product_id,
    serial_num.empty() ? NULL : serial_num.c_str());
Ken Tossell's avatar
Ken Tossell committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

  if (res < 0) {
    uvc_perror(res, "uvc_find_device");
  } else {
    res = uvc_open(dev, &devh);
    if (res < 0) {
      uvc_perror(res, "uvc_open");
    } else {
      puts("Device opened");
      
      uvc_print_diag(devh, stderr);
      
      res = uvc_get_stream_ctrl_format_size(
        devh, &ctrl,
        UVC_COLOR_FORMAT_UNCOMPRESSED, 640, 480, 30);
      
      uvc_print_stream_ctrl(&ctrl, stderr);
      
      if (res < 0) {
	uvc_perror(res, "get_mode");
      } else {
        res = uvc_start_iso_streaming(devh, &ctrl, cb);

        if (res < 0) {
          uvc_perror(res, "start_streaming");
        } else {
          puts("Streaming...");
          ros::spin();
          uvc_stop_streaming(devh);
        }
      }
      
      uvc_close(devh);
      puts("Device closed");
    }
  }
  
  uvc_exit(ctx);
  puts("UVC exited");
  
  return 0;
}