stereo.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"
9 #include "sensor_msgs/CameraInfo.h"
12 
14 
15 using namespace sensor_msgs;
16 
17 
18 struct control_mod {
19  uint32_t id;
20  int32_t val;
21  std::string name;
22 
23  control_mod(uint32_t id, int32_t val, const std::string& name) {
24  this->id = id;
25  this->val = val;
26  this->name = name;
27  }
28 };
29 typedef struct control_mod control_mod_t;
30 
31 
32 /* Rotate an 8-bit, 3-channel image by 180 degrees. */
33 static inline void rotate(unsigned char *dst_chr, unsigned char *src_chr, int pixels) {
34  struct pixel_t {
35  unsigned char r, g, b;
36  };
37 
38  struct pixel_t *src = (pixel_t *) src_chr;
39  struct pixel_t *dst = &(((pixel_t *) dst_chr)[pixels - 1]);
40 
41  for (int i = pixels; i != 0; --i) {
42  *dst = *src;
43  src++;
44  dst--;
45  }
46 }
47 
48 namespace uvc_camera {
49 
50 StereoCamera::StereoCamera(ros::NodeHandle comm_nh, ros::NodeHandle param_nh) :
51  node(comm_nh), pnode(param_nh), it(comm_nh),
52  left_info_mgr(ros::NodeHandle(comm_nh, "left"), "left_camera"),
53  right_info_mgr(ros::NodeHandle(comm_nh, "right"), "right_camera") {
54 
55  /* default config values */
56  width = 640;
57  height = 480;
58  fps = 10;
59  skip_frames = 0;
60  frames_to_skip = 0;
61  left_device = "/dev/video0";
62  right_device = "/dev/video1";
63  frame = "camera";
64  rotate_left = false;
65  rotate_right = false;
66 
67  /* set up information managers */
68  std::string left_url, right_url;
69 
70  pnode.getParam("left/camera_info_url", left_url);
71  pnode.getParam("right/camera_info_url", right_url);
72 
73  left_info_mgr.loadCameraInfo(left_url);
74  right_info_mgr.loadCameraInfo(right_url);
75 
76  /* pull other configuration */
77  pnode.getParam("left/device", left_device);
78  pnode.getParam("right/device", right_device);
79 
80  pnode.getParam("fps", fps);
81  pnode.getParam("skip_frames", skip_frames);
82 
83  pnode.getParam("left/rotate", rotate_left);
84  pnode.getParam("right/rotate", rotate_right);
85 
86  pnode.getParam("width", width);
87  pnode.getParam("height", height);
88 
89  pnode.getParam("frame_id", frame);
90 
91  /* advertise image streams and info streams */
92  left_pub = it.advertise("left/image_raw", 1);
93  right_pub = it.advertise("right/image_raw", 1);
94 
95  left_info_pub = node.advertise<CameraInfo>("left/camera_info", 1);
96  right_info_pub = node.advertise<CameraInfo>("right/camera_info", 1);
97 
98  /* initialize the cameras */
99  cam_left =
101  width, height, fps);
102  cam_left->set_motion_thresholds(100, -1);
103  cam_right =
105  width, height, fps);
107 
108 
109  bool auto_focus;
110  if (pnode.getParam("auto_focus", auto_focus)) {
111  cam_left->set_v4l2_control(V4L2_CID_FOCUS_AUTO, auto_focus, "auto_focus");
112  cam_right->set_v4l2_control(V4L2_CID_FOCUS_AUTO, auto_focus, "auto_focus");
113  }
114 
115  int focus_absolute;
116  if (pnode.getParam("focus_absolute", focus_absolute)) {
117  cam_left->set_v4l2_control(V4L2_CID_FOCUS_ABSOLUTE, focus_absolute, "focus_absolute");
118  cam_right->set_v4l2_control(V4L2_CID_FOCUS_ABSOLUTE, focus_absolute, "focus_absolute");
119  }
120 
121  bool auto_exposure;
122  if (pnode.getParam("auto_exposure", auto_exposure)) {
123  int val;
124  if (auto_exposure) {
125  val = V4L2_EXPOSURE_AUTO;
126  } else {
127  val = V4L2_EXPOSURE_MANUAL;
128  }
129  cam_left->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO, val, "auto_exposure");
130  cam_right->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO, val, "auto_exposure");
131  }
132 
133  int exposure_absolute;
134  if (pnode.getParam("exposure_absolute", exposure_absolute)) {
135  cam_left->set_v4l2_control(V4L2_CID_EXPOSURE_ABSOLUTE, exposure_absolute, "exposure_absolute");
136  cam_right->set_v4l2_control(V4L2_CID_EXPOSURE_ABSOLUTE, exposure_absolute, "exposure_absolute");
137  }
138 
139  int brightness;
140  if (pnode.getParam("brightness", brightness)) {
141  cam_left->set_v4l2_control(V4L2_CID_BRIGHTNESS, brightness, "brightness");
142  cam_right->set_v4l2_control(V4L2_CID_BRIGHTNESS, brightness, "brightness");
143  }
144 
145  int power_line_frequency;
146  if (pnode.getParam("power_line_frequency", power_line_frequency)) {
147  int val;
148  if (power_line_frequency == 0) {
149  val = V4L2_CID_POWER_LINE_FREQUENCY_DISABLED;
150  } else if (power_line_frequency == 50) {
151  val = V4L2_CID_POWER_LINE_FREQUENCY_50HZ;
152  } else if (power_line_frequency == 60) {
153  val = V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
154  } else {
155  printf("power_line_frequency=%d not supported. Using auto.\n", power_line_frequency);
156  val = V4L2_CID_POWER_LINE_FREQUENCY_AUTO;
157  }
158  cam_left->set_v4l2_control(V4L2_CID_POWER_LINE_FREQUENCY, val, "power_line_frequency");
159  cam_right->set_v4l2_control(V4L2_CID_POWER_LINE_FREQUENCY, val, "power_line_frequency");
160  }
161 
162  // TODO:
163  // - add params for
164  // contrast
165  // saturation
166  // hue
167  // white balance temperature, auto and manual
168  // gamma
169  // sharpness
170  // backlight compensation
171  // exposure auto priority
172  // zoom
173  // - add generic parameter list:
174  // [(id0, val0, name0), (id1, val1, name1), ...]
175 
176 
177  /* and turn on the streamer */
178  ok = true;
179  image_thread = boost::thread(boost::bind(&StereoCamera::feedImages, this));
180 }
181 
183  CameraInfoPtr info_left(new CameraInfo(left_info_mgr.getCameraInfo()));
184  CameraInfoPtr info_right(new CameraInfo(right_info_mgr.getCameraInfo()));
185 
186  info_left->header.stamp = time;
187  info_right->header.stamp = time;
188  info_left->header.frame_id = frame;
189  info_right->header.frame_id = frame;
190 
191  left_info_pub.publish(info_left);
192  right_info_pub.publish(info_right);
193 }
194 
195 
197  unsigned int pair_id = 0;
198  while (ok) {
199  unsigned char *frame_left = NULL, *frame_right = NULL;
200  uint32_t bytes_used_left, bytes_used_right;
201 
202  ros::Time capture_time = ros::Time::now();
203 
204  int left_idx = cam_left->grab(&frame_left, bytes_used_left);
205  int right_idx = cam_right->grab(&frame_right, bytes_used_right);
206 
207  /* Read in every frame the camera generates, but only send each
208  * (skip_frames + 1)th frame. This reduces effective frame
209  * rate, processing time and network usage while keeping the
210  * images synchronized within the true framerate.
211  */
212  if (skip_frames == 0 || frames_to_skip == 0) {
213  if (frame_left && frame_right) {
214  ImagePtr image_left(new Image);
215  ImagePtr image_right(new Image);
216 
217  image_left->height = height;
218  image_left->width = width;
219  image_left->step = 3 * width;
220  image_left->encoding = image_encodings::RGB8;
221  image_left->header.stamp = capture_time;
222  image_left->header.seq = pair_id;
223 
224  image_right->height = height;
225  image_right->width = width;
226  image_right->step = 3 * width;
227  image_right->encoding = image_encodings::RGB8;
228  image_right->header.stamp = capture_time;
229  image_right->header.seq = pair_id;
230 
231  image_left->header.frame_id = frame;
232  image_right->header.frame_id = frame;
233 
234  image_left->data.resize(image_left->step * image_left->height);
235  image_right->data.resize(image_right->step * image_right->height);
236 
237  if (rotate_left)
238  rotate(&image_left->data[0], frame_left, width * height);
239  else
240  memcpy(&image_left->data[0], frame_left, width * height * 3);
241 
242  if (rotate_right)
243  rotate(&image_right->data[0], frame_right, width * height);
244  else
245  memcpy(&image_right->data[0], frame_right, width * height * 3);
246 
247  left_pub.publish(image_left);
248  right_pub.publish(image_right);
249 
250  sendInfo(capture_time);
251 
252  ++pair_id;
253  }
254 
256  } else {
257  frames_to_skip--;
258  }
259 
260  if (frame_left)
261  cam_left->release(left_idx);
262  if (frame_right)
263  cam_right->release(right_idx);
264  }
265 }
266 
268  ok = false;
269  image_thread.join();
270  if (cam_left)
271  delete cam_left;
272  if (cam_right)
273  delete cam_right;
274 }
275 
276 };
void release(unsigned buf_idx)
Definition: uvc_cam.cpp:416
camera_info_manager::CameraInfoManager left_info_mgr
Definition: stereocamera.h:33
sensor_msgs::CameraInfo getCameraInfo(void)
void publish(const boost::shared_ptr< M > &message) const
image_transport::Publisher right_pub
Definition: stereocamera.h:35
bool loadCameraInfo(const std::string &url)
uint32_t id
Definition: stereo.cpp:19
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)
boost::thread image_thread
Definition: stereocamera.h:38
void sendInfo(ros::Time time)
Definition: stereo.cpp:182
image_transport::ImageTransport it
Definition: stereocamera.h:25
ros::Publisher left_info_pub
Definition: stereocamera.h:36
ros::Publisher right_info_pub
Definition: stereocamera.h:36
std::string name
Definition: stereo.cpp:21
image_transport::Publisher left_pub
Definition: stereocamera.h:35
void publish(const sensor_msgs::Image &message) const
void set_motion_thresholds(int lum, int count)
Definition: uvc_cam.cpp:492
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle pnode
Definition: stereocamera.h:24
int32_t val
Definition: stereo.cpp:20
ros::NodeHandle node
Definition: stereocamera.h:24
static void rotate(unsigned char *dst_chr, unsigned char *src_chr, int pixels)
Definition: stereo.cpp:33
bool getParam(const std::string &key, std::string &s) const
static Time now()
control_mod(uint32_t id, int32_t val, const std::string &name)
Definition: stereo.cpp:23
uvc_cam::Cam * cam_left
Definition: stereocamera.h:28
camera_info_manager::CameraInfoManager right_info_mgr
Definition: stereocamera.h:33
int grab(unsigned char **frame, uint32_t &bytes_used)
Definition: uvc_cam.cpp:333
uvc_cam::Cam * cam_right
Definition: stereocamera.h:28


uvc_camera
Author(s): Ken Tossell
autogenerated on Mon Jun 10 2019 12:51:49