stereo_camera.cpp
Go to the documentation of this file.
1 
34 
35 namespace avt_vimba_camera {
36 
38 : nh_(nh), nhp_(nhp), it_(nh), left_cam_("left"), right_cam_("right") {
39 
40  // Get the parameters
41  nhp_.param("left_ip", left_ip_, std::string(""));
42  nhp_.param("right_ip", right_ip_, std::string(""));
43  nhp_.param("left_guid", left_guid_, std::string(""));
44  nhp_.param("right_guid", right_guid_, std::string(""));
45  nhp_.param("left_camera_info_url", left_camera_info_url_, std::string(""));
46  nhp_.param("right_camera_info_url", right_camera_info_url_, std::string(""));
47  nhp_.param("show_debug_prints", show_debug_prints_, false);
48  nhp_.param("imgs_buffer_size", imgs_buffer_size_, 2);
49  nhp_.param("max_sec_diff", max_sec_diff_, 0.05);
50 }
51 
53  left_cam_.stop();
54  right_cam_.stop();
55  updater_.broadcast(0, "Device is closed.");
58 }
59 
61  // TODO use nodelets with getMTNodeHandle()
62  // Start Vimba & list all available cameras
63  api_.start();
64 
65  // Set the image publishers before the streaming
66  left_pub_ = it_.advertiseCamera("/stereo_down/left/image_raw", 1);
67  right_pub_ = it_.advertiseCamera("/stereo_down/right/image_raw", 1);
68 
69  // Set the frame callbacks
72 
73  // Publish a hardware message to know & track the state of the cam
75  updater_.broadcast(0, "Device is closed.");
76  double min_freq = 2.0;
77  double max_freq = 20.0;
78  diagnostic_updater::FrequencyStatusParam freq_params(&min_freq, &max_freq, 0.1, 10);
79  double min_stamp = 0.001;
80  double max_stamp = 1.0;
81  diagnostic_updater::TimeStampStatusParam stamp_params(min_stamp, max_stamp);
82  // pub_freq_ = new diagnostic_updater::TopicDiagnostic("Stereo Image Pair", updater_, freq_params, stamp_params);
83  updater_.update();
84 
85  // Set camera info managers
88 
89  pub_left_temp_ = nhp_.advertise<std_msgs::Float64>("left_temp", 1, true);
90  pub_right_temp_ = nhp_.advertise<std_msgs::Float64>("right_temp", 1, true);
91 
92  // Start dynamic_reconfigure & run configure()
93  reconfigure_server_.setCallback(boost::bind(&StereoCamera::configure, this, _1, _2));
94 
95 }
96 
97 void StereoCamera::leftFrameCallback(const FramePtr& vimba_frame_ptr) {
98  ros::Time ros_time = ros::Time::now();
99  if(left_pub_.getNumSubscribers() > 0){
100  sensor_msgs::Image img;
101  if (api_.frameToImage(vimba_frame_ptr, img)){
102  // Publish
103  sensor_msgs::CameraInfo lci = left_info_man_->getCameraInfo();
104  lci.header.stamp = ros_time;
105  img.header.stamp = ros_time;
106  img.header.frame_id = lci.header.frame_id;
107  if (right_pub_.getNumSubscribers() == 0) {
108  left_pub_.publish(img, lci);
109  }
110  else {
111  // Lock right image buffer
112  mutex::scoped_lock r_lock(r_sync_mutex_);
113 
114  // Search a time coincidence with right
115  int idx_r = -1;
116  for (uint i=0; i<r_imgs_buffer_.size(); i++) {
117  double r_stamp = r_imgs_buffer_[i].header.stamp.toSec();
118  if (fabs(r_stamp - ros_time.toSec()) < max_sec_diff_) {
119  idx_r = (int)i;
120  break;
121  }
122  }
123 
124  if (idx_r >= 0) {
125  // Get the corresponding right image
126  sensor_msgs::Image r_img = r_imgs_buffer_[idx_r];
127 
128  // Publish
129  sensor_msgs::CameraInfo rci = right_info_man_->getCameraInfo();
130  r_img.header.stamp = ros_time;
131  lci.header.stamp = ros_time;
132  rci.header.stamp = ros_time;
133  left_pub_.publish(img, lci);
134  right_pub_.publish(r_img, rci);
135 
136  // Delete this right image from buffer
137  r_imgs_buffer_.erase(r_imgs_buffer_.begin(), r_imgs_buffer_.begin() + idx_r + 1);
138  }
139  else {
140  // Add the left image to the buffer
141  mutex::scoped_lock lock(l_sync_mutex_);
142  if (l_imgs_buffer_.size() >= imgs_buffer_size_) {
143  l_imgs_buffer_.erase(l_imgs_buffer_.begin(), l_imgs_buffer_.begin() + 1);
144  }
145  l_imgs_buffer_.push_back(img);
146  }
147  }
148  }
149  else {
150  ROS_WARN_STREAM("Function frameToImage returned 0. No image published.");
151  }
152  }
153 
154  // Publish temp
155  if (pub_left_temp_.getNumSubscribers() > 0) {
156  std_msgs::Float64 msg;
157  msg.data = left_cam_.getDeviceTemp();
158  pub_left_temp_.publish(msg);
159  }
160 
161  updater_.update();
162 }
163 
164 void StereoCamera::rightFrameCallback(const FramePtr& vimba_frame_ptr) {
165  ros::Time ros_time = ros::Time::now();
166  if(right_pub_.getNumSubscribers() > 0){
167  sensor_msgs::Image img;
168  if (api_.frameToImage(vimba_frame_ptr, img)){
169  // Publish
170  sensor_msgs::CameraInfo rci = right_info_man_->getCameraInfo();
171  rci.header.stamp = ros_time;
172  img.header.stamp = ros_time;
173  img.header.frame_id = rci.header.frame_id;
174 
175  // If no left subscribers, publish it
176  if (left_pub_.getNumSubscribers() == 0) {
177  right_pub_.publish(img, rci);
178  }
179  else {
180  // Lock left image buffer
181  mutex::scoped_lock l_lock(l_sync_mutex_);
182 
183  // Search a time coincidence with left
184  int idx_l = -1;
185  for (uint i=0; i<l_imgs_buffer_.size(); i++) {
186  double l_stamp = l_imgs_buffer_[i].header.stamp.toSec();
187  if (fabs(l_stamp - ros_time.toSec()) < max_sec_diff_) {
188  idx_l = (int)i;
189  break;
190  }
191  }
192 
193  if (idx_l >= 0) {
194  // Get the corresponding left image
195  sensor_msgs::Image l_img = l_imgs_buffer_[idx_l];
196 
197  // Publish
198  sensor_msgs::CameraInfo lci = left_info_man_->getCameraInfo();
199  l_img.header.stamp = ros_time;
200  lci.header.stamp = ros_time;
201  rci.header.stamp = ros_time;
202  left_pub_.publish(l_img, lci);
203  right_pub_.publish(img, rci);
204 
205  // Delete this left image from buffer
206  l_imgs_buffer_.erase(l_imgs_buffer_.begin(), l_imgs_buffer_.begin() + idx_l + 1);
207  }
208  else {
209  // Add the right image to the buffer
210  mutex::scoped_lock lock(r_sync_mutex_);
211  if (r_imgs_buffer_.size() >= imgs_buffer_size_) {
212  r_imgs_buffer_.erase(r_imgs_buffer_.begin(), r_imgs_buffer_.begin() + 1);
213  }
214  r_imgs_buffer_.push_back(img);
215  }
216  }
217  }
218  else {
219  ROS_WARN_STREAM("Function frameToImage returned 0. No image published.");
220  }
221  }
222 
223  // Publish temp
225  std_msgs::Float64 msg;
226  msg.data = right_cam_.getDeviceTemp();
228  }
229 }
230 
240 void StereoCamera::configure(StereoConfig& newconfig, uint32_t level) {
241  // Left camera is considered MASTER and right SLAVE
242  try {
243  // The camera already stops & starts acquisition
244  // so there's no problem on changing any feature.
245  if (!left_cam_.isOpened()) {
247  }
248  if (!right_cam_.isOpened()) {
253  }
254  }
255  Config left_config, right_config;
256  copyConfig(newconfig, left_config, right_config);
257  left_cam_.updateConfig(left_config);
258  right_cam_.updateConfig(right_config);
259  updateCameraInfo(newconfig);
260  } catch (const std::exception& e) {
261  ROS_ERROR_STREAM("Error reconfiguring avt_vimba_camera node : " << e.what());
262  }
263 }
264 
266  // left camera
267  lc.frame_id = sc.left_frame_id;
268  lc.trig_timestamp_topic = sc.left_trig_timestamp_topic;
269  lc.acquisition_mode = sc.left_acquisition_mode;
270  lc.acquisition_rate = sc.left_acquisition_rate;
271  lc.trigger_source = sc.left_trigger_source;
272  lc.trigger_mode = sc.left_trigger_mode;
273  lc.trigger_selector = sc.left_trigger_selector;
274  lc.trigger_activation = sc.left_trigger_activation;
275  lc.trigger_delay = sc.left_trigger_delay;
276  lc.exposure = sc.exposure;
277  lc.exposure_auto = sc.exposure_auto;
278  lc.exposure_auto_alg = sc.exposure_auto_alg;
279  lc.exposure_auto_tol = sc.exposure_auto_tol;
280  lc.exposure_auto_max = sc.exposure_auto_max;
281  lc.exposure_auto_min = sc.exposure_auto_min;
282  lc.exposure_auto_outliers = sc.exposure_auto_outliers;
283  lc.exposure_auto_rate = sc.exposure_auto_rate;
284  lc.exposure_auto_target = sc.exposure_auto_target;
285  lc.gain = sc.gain;
286  lc.gain_auto = sc.gain_auto;
287  lc.gain_auto_tol = sc.gain_auto_tol;
288  lc.gain_auto_max = sc.gain_auto_max;
289  lc.gain_auto_min = sc.gain_auto_min;
290  lc.gain_auto_outliers = sc.gain_auto_outliers;
291  lc.gain_auto_rate = sc.gain_auto_rate;
292  lc.gain_auto_target = sc.gain_auto_target;
293  lc.balance_ratio_abs = sc.balance_ratio_abs;
294  lc.balance_ratio_selector = sc.balance_ratio_selector;
295  lc.whitebalance_auto = sc.whitebalance_auto;
296  lc.whitebalance_auto_tol = sc.whitebalance_auto_tol;
297  lc.whitebalance_auto_rate = sc.whitebalance_auto_rate;
298  lc.binning_x = sc.binning_x;
299  lc.binning_y = sc.binning_y;
300  lc.decimation_x = sc.decimation_x;
301  lc.decimation_y = sc.decimation_y;
302  lc.width = sc.width;
303  lc.height = sc.height;
304  lc.roi_width = sc.roi_width;
305  lc.roi_height = sc.roi_height;
306  lc.roi_offset_x = sc.roi_offset_x;
307  lc.roi_offset_y = sc.roi_offset_y;
308  lc.pixel_format = sc.pixel_format;
309  lc.stream_bytes_per_second = sc.stream_bytes_per_second;
310  lc.ptp_mode = sc.left_ptp_mode;
311  lc.sync_in_selector = sc.left_sync_in_selector;
312  lc.sync_out_polarity = sc.left_sync_out_polarity;
313  lc.sync_out_selector = sc.left_sync_out_selector;
314  lc.sync_out_source = sc.left_sync_out_source;
315  // right camera
316  rc.frame_id = sc.right_frame_id;
317  rc.trig_timestamp_topic = sc.right_trig_timestamp_topic;
318  rc.acquisition_mode = sc.right_acquisition_mode;
319  rc.acquisition_rate = sc.right_acquisition_rate;
320  rc.trigger_source = sc.right_trigger_source;
321  rc.trigger_mode = sc.right_trigger_mode;
322  rc.trigger_selector = sc.right_trigger_selector;
323  rc.trigger_activation = sc.right_trigger_activation;
324  rc.trigger_delay = sc.right_trigger_delay;
325  rc.exposure = sc.exposure;
326  rc.exposure_auto = sc.exposure_auto;
327  rc.exposure_auto_alg = sc.exposure_auto_alg;
328  rc.exposure_auto_tol = sc.exposure_auto_tol;
329  rc.exposure_auto_max = sc.exposure_auto_max;
330  rc.exposure_auto_min = sc.exposure_auto_min;
331  rc.exposure_auto_outliers = sc.exposure_auto_outliers;
332  rc.exposure_auto_rate = sc.exposure_auto_rate;
333  rc.exposure_auto_target = sc.exposure_auto_target;
334  rc.gain = sc.gain;
335  rc.gain_auto = sc.gain_auto;
336  rc.gain_auto_tol = sc.gain_auto_tol;
337  rc.gain_auto_max = sc.gain_auto_max;
338  rc.gain_auto_min = sc.gain_auto_min;
339  rc.gain_auto_outliers = sc.gain_auto_outliers;
340  rc.gain_auto_rate = sc.gain_auto_rate;
341  rc.gain_auto_target = sc.gain_auto_target;
342  rc.balance_ratio_abs = sc.balance_ratio_abs;
343  rc.balance_ratio_selector = sc.balance_ratio_selector;
344  rc.whitebalance_auto = sc.whitebalance_auto;
345  rc.whitebalance_auto_tol = sc.whitebalance_auto_tol;
346  rc.whitebalance_auto_rate = sc.whitebalance_auto_rate;
347  rc.binning_x = sc.binning_x;
348  rc.binning_y = sc.binning_y;
349  rc.decimation_x = sc.decimation_x;
350  rc.decimation_y = sc.decimation_y;
351  rc.width = sc.width;
352  rc.height = sc.height;
353  rc.roi_width = sc.roi_width;
354  rc.roi_height = sc.roi_height;
355  rc.roi_offset_x = sc.roi_offset_x;
356  rc.roi_offset_y = sc.roi_offset_y;
357  rc.pixel_format = sc.pixel_format;
358  rc.stream_bytes_per_second = sc.stream_bytes_per_second;
359  rc.ptp_mode = sc.right_ptp_mode;
360  rc.sync_in_selector = sc.right_sync_in_selector;
361  rc.sync_out_polarity = sc.right_sync_out_polarity;
362  rc.sync_out_selector = sc.right_sync_out_selector;
363  rc.sync_out_source = sc.right_sync_out_source;
364 }
365 
367  // Get camera_info from the manager
368  sensor_msgs::CameraInfo left_ci = left_info_man_->getCameraInfo();
369  sensor_msgs::CameraInfo right_ci = right_info_man_->getCameraInfo();
370 
371  // Set the frame id
372  left_ci.header.frame_id = config.left_frame_id;
373  right_ci.header.frame_id = config.right_frame_id;
374 
375  // Set the operational parameters in CameraInfo (binning, ROI)
376  int binning_or_decimation_x = std::max(config.binning_x, config.decimation_x);
377  int binning_or_decimation_y = std::max(config.binning_y, config.decimation_y);
378 
379  left_ci.height = config.height/binning_or_decimation_x;
380  left_ci.width = config.width/binning_or_decimation_y;
381  left_ci.binning_x = 1;
382  left_ci.binning_y = 1;
383 
384  right_ci.height = config.height/binning_or_decimation_x;
385  right_ci.width = config.width/binning_or_decimation_y;
386  right_ci.binning_x = 1;
387  right_ci.binning_y = 1;
388 
389  // ROI in CameraInfo is in unbinned coordinates, need to scale up
390  left_ci.roi.x_offset = config.roi_offset_x/binning_or_decimation_x;
391  left_ci.roi.y_offset = config.roi_offset_y/binning_or_decimation_y;
392  left_ci.roi.height = config.roi_height/binning_or_decimation_x;
393  left_ci.roi.width = config.roi_width/binning_or_decimation_y;
394 
395  right_ci.roi.x_offset = config.roi_offset_x/binning_or_decimation_x;
396  right_ci.roi.y_offset = config.roi_offset_y/binning_or_decimation_y;
397  right_ci.roi.height = config.roi_height/binning_or_decimation_x;
398  right_ci.roi.width = config.roi_width/binning_or_decimation_y;
399 
400  std::string left_camera_info_url, right_camera_info_url;
401  nhp_.getParamCached("left_camera_info_url", left_camera_info_url);
402  nhp_.getParamCached("right_camera_info_url", right_camera_info_url);
403 
404  // set the new URL and load CameraInfo (if any) from it
405  if (left_camera_info_url != left_camera_info_url_) {
406  left_info_man_->setCameraName(config.left_frame_id);
407  if (left_info_man_->validateURL(left_camera_info_url)) {
408  left_info_man_->loadCameraInfo(left_camera_info_url);
409  left_ci = left_info_man_->getCameraInfo();
410  } else {
411  ROS_WARN_STREAM("Camera info URL not valid: " << left_camera_info_url);
412  }
413  }
414 
415  if (right_camera_info_url != right_camera_info_url_) {
416  right_info_man_->setCameraName(config.right_frame_id);
417  if (right_info_man_->validateURL(right_camera_info_url)) {
418  right_info_man_->loadCameraInfo(right_camera_info_url);
419  right_ci = right_info_man_->getCameraInfo();
420  } else {
421  ROS_WARN_STREAM("Camera info URL not valid: " << right_camera_info_url);
422  }
423  }
424 
425  bool lRoiMatchesCalibration = (left_ci.height == config.roi_height
426  && left_ci.width == config.roi_width);
427  bool lResolutionMatchesCalibration = (left_ci.width == config.width
428  && left_ci.height == config.height);
429  // check
430  left_ci.roi.do_rectify = lRoiMatchesCalibration || lResolutionMatchesCalibration;
431 
432  bool rRoiMatchesCalibration = (right_ci.height == config.roi_height
433  && right_ci.width == config.roi_width);
434  bool rResolutionMatchesCalibration = (right_ci.width == config.width
435  && right_ci.height == config.height);
436  // check
437  right_ci.roi.do_rectify = rRoiMatchesCalibration || rResolutionMatchesCalibration;
438 
439  // push the changes to manager
440  left_info_man_->setCameraInfo(left_ci);
441  right_info_man_->setCameraInfo(right_ci);
442 }
443 
444 };
msg
diagnostic_updater::Updater updater_
Definition: stereo_camera.h:74
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
NetPointer< Frame, AVT::VmbAPINET::Frame > FramePtr
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< camera_info_manager::CameraInfoManager > left_info_man_
bool getParamCached(const std::string &key, std::string &s) const
uint32_t getNumSubscribers() const
void setHardwareID(const std::string &hwid)
ReconfigureServer reconfigure_server_
image_transport::ImageTransport it_
Definition: stereo_camera.h:94
std::vector< sensor_msgs::Image > r_imgs_buffer_
void setCallback(frameCallbackFunc callback=&avt_vimba_camera::AvtVimbaCamera::defaultFrameCallback)
void leftFrameCallback(const FramePtr &vimba_frame_ptr)
bool frameToImage(const FramePtr vimba_frame_ptr, sensor_msgs::Image &image)
boost::shared_ptr< camera_info_manager::CameraInfoManager > right_info_man_
avt_vimba_camera::AvtVimbaCameraStereoConfig StereoConfig
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
StereoCamera(ros::NodeHandle nh, ros::NodeHandle nhp)
void configure(StereoConfig &newconfig, uint32_t level)
std::vector< sensor_msgs::Image > l_imgs_buffer_
void copyConfig(StereoConfig &sc, Config &lc, Config &rc)
void start(std::string ip_str, std::string guid_str, bool debug_prints=true)
image_transport::CameraPublisher left_pub_
Definition: stereo_camera.h:97
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
uint32_t getNumSubscribers() const
static Time now()
void rightFrameCallback(const FramePtr &vimba_frame_ptr)
void broadcast(int lvl, const std::string msg)
#define ROS_ERROR_STREAM(args)
avt_vimba_camera::AvtVimbaCameraConfig Config
image_transport::CameraPublisher right_pub_
Definition: stereo_camera.h:98
void updateCameraInfo(const StereoConfig &config)


avt_vimba_camera
Author(s): Miquel Massot , Allied Vision Technologies
autogenerated on Wed Jun 5 2019 22:22:40