camera.cpp
Go to the documentation of this file.
1 #include <boost/thread.hpp>
2 
3 #include <ros/ros.h>
4 #include <ros/time.h>
5 
6 #include "uvc_cam/uvc_cam.h"
7 #include "sensor_msgs/Image.h"
8 #include "sensor_msgs/CompressedImage.h"
10 #include "sensor_msgs/CameraInfo.h"
13 
14 #include "uvc_camera/camera.h"
15 
16 using namespace sensor_msgs;
17 
18 namespace uvc_camera {
19 
20 Camera::Camera(ros::NodeHandle _comm_nh, ros::NodeHandle _param_nh) :
21  node(_comm_nh), pnode(_param_nh), it(_comm_nh),
22  info_mgr(_comm_nh, "camera"), cam(0) {
23 
24  /* default config values */
25  width = 640;
26  height = 480;
27  fps = 10;
28  skip_frames = 0;
29  frames_to_skip = 0;
30  device = "/dev/video0";
31  frame = "camera";
32  rotate = false;
33  format = "rgb";
34 
35  /* set up information manager */
36  std::string url, camera_name;
37  pnode.getParam("camera_info_url", url);
38  pnode.param<std::string>("camera_name", camera_name, "camera");
39 
40  info_mgr.setCameraName(camera_name);
42 
43  /* pull other configuration */
44  pnode.getParam("device", device);
45 
46  pnode.getParam("fps", fps);
47  pnode.getParam("skip_frames", skip_frames);
48 
49  pnode.getParam("width", width);
50  pnode.getParam("height", height);
51 
52  pnode.getParam("frame_id", frame);
53 
54  pnode.getParam("format", format);
55 
56  /* advertise image streams and info streams */
57  if (format != "jpeg")
58  pub = it.advertise("image_raw", 1);
59  else
60  pubjpeg = node.advertise<CompressedImage>("image_raw/compressed", 1);
61 
62  info_pub = node.advertise<CameraInfo>("camera_info", 1);
63 
64  /* initialize the cameras */
66  if (format == "jpeg")
68  cam = new uvc_cam::Cam(device.c_str(), mode, width, height, fps);
69  cam->set_motion_thresholds(100, -1);
70 
71  bool auto_focus;
72  if (pnode.getParam("auto_focus", auto_focus)) {
73  cam->set_v4l2_control(V4L2_CID_FOCUS_AUTO, auto_focus, "auto_focus");
74  }
75 
76  int focus_absolute;
77  if (pnode.getParam("focus_absolute", focus_absolute)) {
78  cam->set_v4l2_control(V4L2_CID_FOCUS_ABSOLUTE, focus_absolute, "focus_absolute");
79  }
80 
81  bool auto_exposure;
82  if (pnode.getParam("auto_exposure", auto_exposure)) {
83  int val;
84  if (auto_exposure) {
85  val = V4L2_EXPOSURE_AUTO;
86  } else {
87  val = V4L2_EXPOSURE_MANUAL;
88  }
89  cam->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO, val, "auto_exposure");
90  }
91 
92  int exposure_auto_priority;
93  if (pnode.getParam("exposure_auto_priority", exposure_auto_priority)) {
94  cam->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO_PRIORITY, exposure_auto_priority, "exposure_auto_priority");
95  }
96 
97  int exposure_absolute;
98  if (pnode.getParam("exposure_absolute", exposure_absolute)) {
99  cam->set_v4l2_control(V4L2_CID_EXPOSURE_ABSOLUTE, exposure_absolute, "exposure_absolute");
100  }
101 
102  int exposure;
103  if (pnode.getParam("exposure", exposure)) {
104  cam->set_v4l2_control(V4L2_CID_EXPOSURE, exposure, "exposure");
105  }
106 
107  int brightness;
108  if (pnode.getParam("brightness", brightness)) {
109  cam->set_v4l2_control(V4L2_CID_BRIGHTNESS, brightness, "brightness");
110  }
111 
112  int power_line_frequency;
113  if (pnode.getParam("power_line_frequency", power_line_frequency)) {
114  int val;
115  if (power_line_frequency == 0) {
116  val = V4L2_CID_POWER_LINE_FREQUENCY_DISABLED;
117  } else if (power_line_frequency == 50) {
118  val = V4L2_CID_POWER_LINE_FREQUENCY_50HZ;
119  } else if (power_line_frequency == 60) {
120  val = V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
121  } else {
122  printf("power_line_frequency=%d not supported. Using auto.\n", power_line_frequency);
123  val = V4L2_CID_POWER_LINE_FREQUENCY_AUTO;
124  }
125  cam->set_v4l2_control(V4L2_CID_POWER_LINE_FREQUENCY, val, "power_line_frequency");
126  }
127 
128  int contrast;
129  if (pnode.getParam("contrast", contrast)) {
130  cam->set_v4l2_control(V4L2_CID_CONTRAST, contrast, "contrast");
131  }
132 
133  int saturation;
134  if (pnode.getParam("saturation", saturation)) {
135  cam->set_v4l2_control(V4L2_CID_SATURATION, saturation, "saturation");
136  }
137 
138  int hue;
139  if (pnode.getParam("hue", hue)) {
140  cam->set_v4l2_control(V4L2_CID_HUE, hue, "hue");
141  }
142 
143  bool auto_white_balance;
144  if (pnode.getParam("auto_white_balance", auto_white_balance)) {
145  cam->set_v4l2_control(V4L2_CID_AUTO_WHITE_BALANCE, auto_white_balance, "auto_white_balance");
146  }
147 
148  int white_balance_tmp;
149  if (pnode.getParam("white_balance_temperature", white_balance_tmp)) {
150  cam->set_v4l2_control(V4L2_CID_WHITE_BALANCE_TEMPERATURE, white_balance_tmp, "white_balance_temperature");
151  }
152 
153  int gamma;
154  if (pnode.getParam("gamma", gamma)) {
155  cam->set_v4l2_control(V4L2_CID_GAMMA, gamma, "gamma");
156  }
157 
158  int sharpness;
159  if (pnode.getParam("sharpness", sharpness)) {
160  cam->set_v4l2_control(V4L2_CID_SHARPNESS, sharpness, "sharpness");
161  }
162 
163  int backlight_comp;
164  if (pnode.getParam("backlight_compensation", backlight_comp)) {
165  cam->set_v4l2_control(V4L2_CID_BACKLIGHT_COMPENSATION, backlight_comp, "backlight_compensation");
166  }
167 
168  bool auto_gain;
169  if (pnode.getParam("auto_gain", auto_gain)) {
170  cam->set_v4l2_control(V4L2_CID_AUTOGAIN, auto_gain, "auto_gain");
171  }
172 
173  int gain;
174  if (pnode.getParam("gain", gain)) {
175  cam->set_v4l2_control(V4L2_CID_GAIN, gain, "gain");
176  }
177 
178  bool h_flip;
179  if (pnode.getParam("horizontal_flip", h_flip)) {
180  cam->set_v4l2_control(V4L2_CID_HFLIP, h_flip, "horizontal_flip");
181  }
182 
183  bool v_flip;
184  if (pnode.getParam("vertical_flip", v_flip)) {
185  cam->set_v4l2_control(V4L2_CID_VFLIP, v_flip, "vertical_flip");
186  }
187 
188  // TODO:
189  // - zoom absolute, zoom relative and zoom continuous controls
190  // - add generic parameter list:
191  // [(id0, val0, name0), (id1, val1, name1), ...
192 
193  /* and turn on the streamer */
194  ok = true;
195  image_thread = boost::thread(boost::bind(&Camera::feedImages, this));
196  }
197 
198  void Camera::sendInfo(ImagePtr &image, ros::Time time) {
199  CameraInfoPtr info(new CameraInfo(info_mgr.getCameraInfo()));
200 
201  /* Throw out any CamInfo that's not calibrated to this camera mode */
202  if (info->K[0] != 0.0 &&
203  (image->width != info->width
204  || image->height != info->height)) {
205  info.reset(new CameraInfo());
206  }
207 
208  /* If we don't have a calibration, set the image dimensions */
209  if (info->K[0] == 0.0) {
210  info->width = image->width;
211  info->height = image->height;
212  }
213 
214  info->header.stamp = time;
215  info->header.frame_id = frame;
216 
217  info_pub.publish(info);
218  }
219 
221  CameraInfoPtr info(new CameraInfo(info_mgr.getCameraInfo()));
222  info->header.stamp = time;
223  info->header.frame_id = frame;
224 
225  info_pub.publish(info);
226  }
227 
229  unsigned int pair_id = 0;
230  while (ok) {
231  unsigned char *img_frame = NULL;
232  uint32_t bytes_used;
233 
234  ros::Time capture_time = ros::Time::now();
235 
236  int idx = cam->grab(&img_frame, bytes_used);
237 
238  /* Read in every frame the camera generates, but only send each
239  * (skip_frames + 1)th frame. It's set up this way just because
240  * this is based on Stereo...
241  */
242  if (skip_frames == 0 || frames_to_skip == 0) {
243  if (img_frame && format != "jpeg") {
244  ImagePtr image(new Image);
245 
246  image->height = height;
247  image->width = width;
248  image->step = 3 * width;
249  image->encoding = image_encodings::RGB8;
250 
251  image->header.stamp = capture_time;
252  image->header.seq = pair_id;
253 
254  image->header.frame_id = frame;
255 
256  image->data.resize(image->step * image->height);
257 
258  memcpy(&image->data[0], img_frame, width*height * 3);
259 
260  pub.publish(image);
261 
262  sendInfo(image, capture_time);
263 
264  ++pair_id;
265  } else if (img_frame && format == "jpeg") {
266  CompressedImagePtr image(new CompressedImage);
267 
268  image->header.stamp = capture_time;
269  image->header.seq = pair_id;
270 
271  image->header.frame_id = frame;
272 
273  image->data.resize(bytes_used);
274 
275  memcpy(&image->data[0], img_frame, bytes_used);
276 
277  pubjpeg.publish(image);
278 
279  sendInfoJpeg(capture_time);
280 
281  ++pair_id;
282  }
283 
285  } else {
286  frames_to_skip--;
287  }
288 
289  if (img_frame) cam->release(idx);
290  }
291  }
292 
294  ok = false;
295  image_thread.join();
296  if (cam) delete cam;
297  }
298 
299 
300 };
301 
image_transport::Publisher pub
Definition: camera.h:29
camera_info_manager::CameraInfoManager info_mgr
Definition: camera.h:27
void release(unsigned buf_idx)
Definition: uvc_cam.cpp:416
sensor_msgs::CameraInfo getCameraInfo(void)
void publish(const boost::shared_ptr< M > &message) const
bool loadCameraInfo(const std::string &url)
bool set_v4l2_control(int id, int value, const std::string &name)
Definition: uvc_cam.cpp:447
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
std::string format
Definition: camera.h:24
ros::Publisher info_pub
Definition: camera.h:31
void publish(const sensor_msgs::Image &message) const
boost::thread image_thread
Definition: camera.h:34
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::string frame
Definition: camera.h:24
ros::Publisher pubjpeg
Definition: camera.h:30
void set_motion_thresholds(int lum, int count)
Definition: uvc_cam.cpp:492
void sendInfo(sensor_msgs::ImagePtr &image, ros::Time time)
Definition: camera.cpp:198
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle pnode
Definition: camera.h:19
image_transport::ImageTransport it
Definition: camera.h:20
uvc_cam::Cam * cam
Definition: camera.h:33
ros::NodeHandle node
Definition: camera.h:19
int frames_to_skip
Definition: camera.h:23
std::string device
Definition: camera.h:24
bool getParam(const std::string &key, std::string &s) const
void sendInfoJpeg(ros::Time time)
Definition: camera.cpp:220
static Time now()
int grab(unsigned char **frame, uint32_t &bytes_used)
Definition: uvc_cam.cpp:333
bool setCameraName(const std::string &cname)


uvc_camera
Author(s): Ken Tossell
autogenerated on Sat Jun 8 2019 17:54:12