ensenso_driver.cpp
Go to the documentation of this file.
1 // ROS headers
2 #include <ros/ros.h>
3 //tf to rgb
4 #include <tf2_ros/transform_broadcaster.h>
5 
6 #include <sensor_msgs/Image.h>
7 #include <cv_bridge/cv_bridge.h>
8 #include <pcl_ros/point_cloud.h>
9 // Conversions
10 #include <eigen_conversions/eigen_msg.h>
11 // Ensenso grabber
12 #include <ensenso/ensenso_grabber.h>
13 // Image transport
14 #include <image_transport/image_transport.h>
15 #include <sensor_msgs/CameraInfo.h>
16 #include <sensor_msgs/image_encodings.h>
17 // Dynamic reconfigure
18 #include <dynamic_reconfigure/server.h>
19 #include <ensenso/CameraParametersConfig.h>
20 // Messages
21 #include <ensenso/RawStereoPattern.h>
22 #include <geometry_msgs/PoseStamped.h>
23 #include <sensor_msgs/PointCloud2.h>
24 #include <std_msgs/String.h>
25 // Services
26 #include <ensenso/CalibrateHandEye.h>
27 #include <ensenso/CollectPattern.h>
28 #include <ensenso/EstimatePatternPose.h>
29 // boost
30 #include <boost/thread/thread.hpp>
31 
32 // Typedefs
33 typedef std::pair<pcl::PCLGenImage<pcl::uint8_t>, pcl::PCLGenImage<pcl::uint8_t> > PairOfImages;
34 typedef pcl::PointXYZRGBA PointXYZRGBA;
35 typedef pcl::PointCloud<PointXYZRGBA> PointCloudXYZRGBA;
36 typedef pcl::PointXYZ PointXYZ;
37 typedef pcl::PointCloud<PointXYZ> PointCloudXYZ;
38 
39 
40 class EnsensoDriver
41 {
42  private:
43  // ROS
44  ros::NodeHandle nh_, nh_private_;
45  ros::ServiceServer pattern_srv_;
46  ros::ServiceServer collect_srv_;
47  ros::ServiceServer calibrate_srv_;
48  dynamic_reconfigure::Server<ensenso::CameraParametersConfig> reconfigure_server_;
49  // Images
50  image_transport::ImageTransport it_;
51  image_transport::CameraPublisher rgb_raw_pub_;
52  image_transport::CameraPublisher l_raw_pub_;
53  image_transport::CameraPublisher r_raw_pub_;
54  image_transport::CameraPublisher depth_pub_;
55  image_transport::Publisher rgb_rectified_pub_;
56  image_transport::Publisher l_rectified_pub_;
57  image_transport::Publisher r_rectified_pub_;
58 
59  // Publishers
60  ros::Publisher cloud_pub_;
61  ros::Publisher pattern_pose_pub_;
62  ros::Publisher pattern_raw_pub_;
63  // Streaming configuration
64  bool rgb_available_;
65  bool enable_cloud_;
66  bool enable_images_;
67  bool enable_depth_;
68  bool is_streaming_cloud_;
69  bool is_streaming_depth_;
70  bool is_streaming_images_;
71  bool find_pattern_;
72  bool stream_calib_pattern_;
73  int trigger_mode_;
74  // TF
75  std::string camera_frame_id_;
76  std::string rgb_camera_frame_id_;
77  tf2_ros::TransformBroadcaster tf_br_;
78  ros::Timer tf_publisher_;
79  // Ensenso grabber
80  boost::signals2::connection cloud_connection_;
81  boost::signals2::connection image_connection_;
82  boost::signals2::connection depth_connection_;
83  pcl::EnsensoGrabber::Ptr ensenso_ptr_;
84 
85  public:
86  EnsensoDriver():
87  rgb_available_(false),
88  enable_cloud_(false),
89  enable_images_(false),
90  enable_depth_(false),
91  is_streaming_images_(false),
92  is_streaming_cloud_(false),
93  is_streaming_depth_(false),
94  stream_calib_pattern_(false),
95  find_pattern_(true),
96  trigger_mode_(-1),
97  nh_private_("~"),
98  it_(nh_)
99  {
100  // Read parameters
101  std::string serial, monoserial;
102  nh_private_.param(std::string("serial"), serial, std::string("150534"));
103  if (!nh_private_.hasParam("serial"))
104  ROS_WARN_STREAM("Parameter [~serial] not found, using default: " << serial);
105  nh_private_.param(std::string("monoserial"), monoserial, std::string("4103203953"));
106  if (!nh_private_.hasParam("monoserial"))
107  ROS_WARN_STREAM("Parameter [~monoserial] not found, using default: " << monoserial);
108  nh_private_.param("camera_frame_id", camera_frame_id_, std::string("ensenso_optical_frame"));
109  if (!nh_private_.hasParam("camera_frame_id"))
110  ROS_WARN_STREAM("Parameter [~camera_frame_id] not found, using default: " << camera_frame_id_);
111  nh_private_.param("rgb_camera_frame_id", rgb_camera_frame_id_, std::string("ensenso_rgb_optical_frame"));
112  if (!nh_private_.hasParam("rgb_camera_frame_id"))
113  ROS_WARN_STREAM("Parameter [~rgb_camera_frame_id] not found, using default: " << rgb_camera_frame_id_);
114  nh_private_.param("stream_calib_pattern", stream_calib_pattern_, false);
115  if (!nh_private_.hasParam("stream_calib_pattern"))
116  ROS_WARN_STREAM("Parameter [~stream_calib_pattern] not found, using default: " << (stream_calib_pattern_ ? "TRUE":"FALSE"));
117  // Advertise topics
118  ros::SubscriberStatusCallback cloud_rssc = boost::bind(&EnsensoDriver::cloudSubscribeCallback, this);
119  image_transport::SubscriberStatusCallback image_issc = boost::bind(&EnsensoDriver::imagesSubscribeCallback, this);
120  ros::SubscriberStatusCallback image_rssc = boost::bind(&EnsensoDriver::imagesSubscribeCallback, this);
121  image_transport::SubscriberStatusCallback depth_issc = boost::bind(&EnsensoDriver::depthSubscribeCallback, this);
122  ros::SubscriberStatusCallback depth_rssc = boost::bind(&EnsensoDriver::depthSubscribeCallback, this);
123 
124  l_raw_pub_ = it_.advertiseCamera("left/image_raw", 1, image_issc, image_issc, image_rssc, image_rssc);
125  r_raw_pub_ = it_.advertiseCamera("right/image_raw", 1, image_issc, image_issc, image_rssc, image_rssc);
126 
127  l_rectified_pub_ = it_.advertise("left/image_rect", 1, image_issc, image_issc);
128  r_rectified_pub_ = it_.advertise("right/image_rect", 1, image_issc, image_issc);
129 
130  depth_pub_ = it_.advertiseCamera("depth/image_rect", 1, depth_issc, depth_issc, depth_rssc, depth_rssc);
131 
132  cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2 >("depth/points", 1, cloud_rssc, cloud_rssc);
133 
134  // Initialize Ensenso
135  ensenso_ptr_.reset(new pcl::EnsensoGrabber);
136  ensenso_ptr_->openDevice(serial);
137  try
138  {
139  if (ensenso_ptr_->openMonoDevice(monoserial))
140  {
141  rgb_available_ = true;
142  ROS_INFO("Found RGB camera");
143  image_transport::SubscriberStatusCallback image_issc = boost::bind(&EnsensoDriver::imagesSubscribeCallback, this);
144  ros::SubscriberStatusCallback image_rssc = boost::bind(&EnsensoDriver::imagesSubscribeCallback, this);
145  rgb_raw_pub_ = it_.advertiseCamera("rgb/image_raw", 1, image_issc, image_issc, image_rssc, image_rssc);
146  rgb_rectified_pub_ = it_.advertise("rgb/image_rect_color", 1, image_issc, image_issc);
147  tf_publisher_ = nh_.createTimer(ros::Duration(0.5), boost::bind(&EnsensoDriver::publishTF, this));
148  ensenso_ptr_->setUseRGB(true);
149  }
150  }
151  catch (pcl::IOException e)
152  {
153  rgb_available_ = false;
154  ROS_INFO("No RGB camera found");
155  }
156 
157  ensenso_ptr_->openTcpPort();
158  ensenso_ptr_->storeCalibrationPattern(stream_calib_pattern_);
159  // Start dynamic reconfigure server
160  dynamic_reconfigure::Server<ensenso::CameraParametersConfig>::CallbackType f;
161  f = boost::bind(&EnsensoDriver::cameraParametersCallback, this, _1, _2);
162  reconfigure_server_.setCallback(f);
163  // Start the camera.
164  ensenso_ptr_->start();
165  // Advertise services
166  ROS_INFO("Finished [ensenso_driver] initialization");
167  }
168 
169  ~EnsensoDriver()
170  {
171  cloud_connection_.disconnect();
172  image_connection_.disconnect();
173  depth_connection_.disconnect();
174  ensenso_ptr_->closeDevices();
175  ensenso_ptr_->closeTcpPort();
176  }
177 
178  bool calibrateHandEyeCB(ensenso::CalibrateHandEye::Request& req, ensenso::CalibrateHandEye::Response &res)
179  {
180  bool was_running = ensenso_ptr_->isRunning();
181  if (was_running)
182  ensenso_ptr_->stop();
183  // Check consistency between robot and pattern poses
184  if ( req.robot_poses.poses.size() != ensenso_ptr_->getPatternCount() )
185  {
186  ROS_WARN("The number of robot_poses differs from the pattern count in the camera buffer");
187  if (was_running)
188  ensenso_ptr_->start();
189  return true;
190  }
191  // Convert poses to Eigen::Affine3d
192  std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > robot_eigen_list;
193  for (size_t i = 0; i < req.robot_poses.poses.size(); i++) {
194  Eigen::Affine3d pose;
195  tf::poseMsgToEigen(req.robot_poses.poses[i], pose);
196  robot_eigen_list.push_back(pose);
197  }
198  // Calibrate
199  Eigen::Affine3d camera_seed, pattern_seed, estimated_camera_pose, estimated_pattern_pose;
200  tf::poseMsgToEigen(req.camera_seed, camera_seed);
201  tf::poseMsgToEigen(req.pattern_seed, pattern_seed);
202  ROS_INFO("calibrateHandEye: It may take up to 5 minutes...");
203  res.success = ensenso_ptr_->calibrateHandEye(robot_eigen_list, camera_seed, pattern_seed,
204  req.setup, estimated_camera_pose, estimated_pattern_pose, res.iterations,
205  res.reprojection_error);
206  if (res.success)
207  {
208  ROS_INFO("Calibration computation finished");
209  tf::poseEigenToMsg(estimated_camera_pose, res.estimated_camera_pose);
210  tf::poseEigenToMsg(estimated_pattern_pose, res.estimated_pattern_pose);
211  }
212  if (was_running)
213  ensenso_ptr_->start();
214  return true;
215  }
216 
217  void cameraParametersCallback(ensenso::CameraParametersConfig &config, uint32_t level)
218  {
219  // Process enumerators
220  std::string trigger_mode, profile;
221  switch (config.TriggerMode)
222  {
223  case 0:
224  trigger_mode = "Software";
225  break;
226  case 1:
227  trigger_mode = "FallingEdge";
228  break;
229  case 2:
230  trigger_mode = "RisingEdge";
231  break;
232  default:
233  trigger_mode = "Software";
234  }
235  switch (config.OptimizationProfile)
236  {
237  case 0:
238  profile = "Aligned";
239  break;
240  case 1:
241  profile = "Diagonal";
242  break;
243  case 2:
244  profile = "AlignedAndDiagonal";
245  break;
246  default:
247  profile = "AlignedAndDiagonal";
248  }
249  ROS_DEBUG("---");
250  ROS_DEBUG("Capture Parameters");
251  ROS_DEBUG_STREAM("AutoBlackLevel: " << std::boolalpha << config.AutoBlackLevel);
252  ROS_DEBUG_STREAM("AutoExposure: " << std::boolalpha << config.AutoExposure);
253  ROS_DEBUG_STREAM("AutoGain: " << std::boolalpha << config.AutoGain);
254  ROS_DEBUG_STREAM("Binning: " << config.Binning);
255  ROS_DEBUG_STREAM("BlackLevelOffset: " << config.BlackLevelOffset);
256  ROS_DEBUG_STREAM("Exposure: " << config.Exposure);
257  ROS_DEBUG_STREAM("FlexView: " << std::boolalpha << config.FlexView);
258  ROS_DEBUG_STREAM("FlexViewImages: " << config.FlexViewImages);
259  ROS_DEBUG_STREAM("FrontLight: " << std::boolalpha << config.FrontLight);
260  ROS_DEBUG_STREAM("Gain: " << config.Gain);
261  ROS_DEBUG_STREAM("GainBoost: " << std::boolalpha << config.GainBoost);
262  ROS_DEBUG_STREAM("HardwareGamma: " << std::boolalpha << config.HardwareGamma);
263  ROS_DEBUG_STREAM("Hdr: " << std::boolalpha << config.Hdr);
264  ROS_DEBUG_STREAM("PixelClock: " << config.PixelClock);
265  ROS_DEBUG_STREAM("Projector: " << std::boolalpha << config.Projector);
266  ROS_DEBUG_STREAM("TargetBrightness: " << config.TargetBrightness);
267  ROS_DEBUG_STREAM("TriggerMode: " << trigger_mode);
268  ROS_DEBUG_STREAM("RGBTriggerDelay: " << config.RGBTriggerDelay);
269  ROS_DEBUG_STREAM("DisparityMapAOI: " << std::boolalpha << config.DisparityMapAOI);
270  ROS_DEBUG("Stereo Matching Parameters");
271  ROS_DEBUG_STREAM("MinimumDisparity: " << config.MinimumDisparity);
272  ROS_DEBUG_STREAM("NumberOfDisparities: " << config.NumberOfDisparities);
273  ROS_DEBUG_STREAM("OptimizationProfile: " << profile);
274  ROS_DEBUG_STREAM("Scaling: " << config.Scaling);
275  ROS_DEBUG("Advanced Matching Parameters");
276  ROS_DEBUG_STREAM("DepthChangeCost: " << config.DepthChangeCost);
277  ROS_DEBUG_STREAM("DepthStepCost: " << config.DepthStepCost);
278  ROS_DEBUG_STREAM("ShadowingThreshold: " << config.ShadowingThreshold);
279  ROS_DEBUG("Postprocessing Parameters");
280  ROS_DEBUG_STREAM("Find Pattern: " << std::boolalpha << config.FindPattern);
281  if (!config.FindPattern)
282  {
283  ROS_WARN_STREAM("The calibration pattern will not be searched for, calibration will not work.");
284  }
285  ROS_DEBUG_STREAM("UniquenessRatio: " << config.UniquenessRatio);
286  ROS_DEBUG_STREAM("MedianFilterRadius: "<< config.MedianFilterRadius);
287  ROS_DEBUG_STREAM("SpeckleComponentThreshold: "<< config.SpeckleComponentThreshold);
288  ROS_DEBUG_STREAM("SpeckleRegionSize: "<< config.SpeckleRegionSize);
289  ROS_DEBUG_STREAM("FillBorderSpread: "<< config.FillBorderSpread);
290  ROS_DEBUG_STREAM("FillRegionSize: " << config.FillRegionSize);
291  ROS_DEBUG("Render Parameters");
292  ROS_DEBUG_STREAM("SurfaceConnectivity: " << std::boolalpha << config.SurfaceConnectivity);
293  ROS_DEBUG_STREAM("NearPlane: " << std::boolalpha << config.NearPlane);
294  ROS_DEBUG_STREAM("FarPlane: " << std::boolalpha << config.FarPlane);
295  ROS_DEBUG_STREAM("UseOpenGL: " << std::boolalpha << config.UseOpenGL);
296  ROS_DEBUG("Stream Parameters");
297  ROS_DEBUG_STREAM("Cloud: " << std::boolalpha << config.Cloud);
298  ROS_DEBUG_STREAM("Images: " << std::boolalpha << config.Images);
299  ROS_DEBUG_STREAM("Depth: " << std::boolalpha << config.Depth);
300  ROS_DEBUG("CUDA Parameters");
301  #ifdef CUDA_IMPLEMENTED
302  ROS_DEBUG_STREAM("Use CUDA: " << std::boolalpha << config.EnableCUDA);
303  #else
304  ROS_DEBUG_STREAM("CUDA is not supported. Upgrade EnsensoSDK to Version >= 2.1.7 in order to use CUDA.");
305  #endif
306  ROS_DEBUG("---");
307 
308  enable_cloud_ = config.Cloud;
309  enable_images_ = config.Images;
310  enable_depth_ = config.Depth;
311 
312  //advertise topics only when parameters are set accordingly
313  if (config.FindPattern && !find_pattern_)
314  {
315  pattern_raw_pub_ = nh_.advertise<ensenso::RawStereoPattern> ("pattern/stereo", 1, false);
316  pattern_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped> ("pattern/pose", 1, false);
317  calibrate_srv_ = nh_.advertiseService("calibrate_handeye", &EnsensoDriver::calibrateHandEyeCB, this);
318  pattern_srv_ = nh_.advertiseService("estimate_pattern_pose", &EnsensoDriver::estimatePatternPoseCB, this);
319  collect_srv_ = nh_.advertiseService("collect_pattern", &EnsensoDriver::collectPatternCB, this);
320  }
321  find_pattern_ = config.FindPattern;
322  // Capture parameters
323  ensenso_ptr_->setAutoBlackLevel(config.AutoBlackLevel);
324  ensenso_ptr_->setAutoExposure(config.AutoExposure);
325  ensenso_ptr_->setAutoGain(config.AutoGain);
326  ensenso_ptr_->setBlackLevelOffset(config.BlackLevelOffset);
327  ensenso_ptr_->setExposure(config.Exposure);
328  ensenso_ptr_->setFrontLight(config.FrontLight);
329  ensenso_ptr_->setGain(config.Gain);
330  ensenso_ptr_->setGainBoost(config.GainBoost);
331  ensenso_ptr_->setHardwareGamma(config.HardwareGamma);
332  ensenso_ptr_->setHdr(config.Hdr);
333  ensenso_ptr_->setPixelClock(config.PixelClock);
334  ensenso_ptr_->setProjector(config.Projector);
335  ensenso_ptr_->setRGBTriggerDelay(config.RGBTriggerDelay);
336  ensenso_ptr_->setTargetBrightness(config.TargetBrightness);
337  ensenso_ptr_->setTriggerMode(trigger_mode);
338  ensenso_ptr_->setUseDisparityMapAreaOfInterest(config.DisparityMapAOI);
339  // Flexview and binning only work in 'Software' trigger mode and with the projector on
340  if (trigger_mode.compare("Software") == 0 && config.Projector)
341  {
342  ensenso_ptr_->setBinning(config.Binning);
343  ensenso_ptr_->setFlexView(config.FlexView, config.FlexViewImages);
344  }
345  // Stereo parameters
346  ensenso_ptr_->setMinimumDisparity(config.MinimumDisparity);
347  ensenso_ptr_->setNumberOfDisparities(config.NumberOfDisparities);
348  ensenso_ptr_->setOptimizationProfile(profile);
349  ensenso_ptr_->setScaling(config.Scaling);
350  ensenso_ptr_->setDepthChangeCost(config.DepthChangeCost);
351  ensenso_ptr_->setDepthStepCost(config.DepthStepCost);
352  ensenso_ptr_->setShadowingThreshold(config.ShadowingThreshold);
353  //Postprocessing parameters
354  ensenso_ptr_->setUniquenessRatio(config.UniquenessRatio);
355  ensenso_ptr_->setMedianFilterRadius(config.MedianFilterRadius);
356  ensenso_ptr_->setSpeckleComponentThreshold(config.SpeckleComponentThreshold);
357  ensenso_ptr_->setSpeckleRegionSize(config.SpeckleRegionSize);
358  ensenso_ptr_->setFillBorderSpread(config.FillBorderSpread);
359  ensenso_ptr_->setFillRegionSize(config.FillRegionSize);
360  ensenso_ptr_->setFindPattern(config.FindPattern);
361  //Render parameters
362  ensenso_ptr_->setSurfaceConnectivity(config.SurfaceConnectivity);
363  ensenso_ptr_->setNearPlane(config.NearPlane);
364  ensenso_ptr_->setFarPlane(config.FarPlane);
365  ensenso_ptr_->setUseOpenGL(config.UseOpenGL);
366  //CUDA parameter
367  #ifdef CUDA_IMPLEMENTED
368  ensenso_ptr_->setEnableCUDA(config.EnableCUDA);
369  #endif
370  // Streaming parameters - only request rgb when available
371  if (trigger_mode_ != config.TriggerMode)
372  {
373  trigger_mode_ = config.TriggerMode;
374  if (ensenso_ptr_->isRunning())
375  {
376  ensenso_ptr_->stop();
377  ensenso_ptr_->start();
378  }
379  }
380  //check if someone is subscribed and start!
381  depthSubscribeCallback();
382  imagesSubscribeCallback();
383  cloudSubscribeCallback();
384  }
385 
386  bool collectPatternCB(ensenso::CollectPattern::Request& req, ensenso::CollectPattern::Response &res)
387  {
388  bool was_running = ensenso_ptr_->isRunning();
389  if (was_running)
390  ensenso_ptr_->stop();
391  // Check consistency
392  if (!req.decode && req.grid_spacing <= 0)
393  {
394  ROS_WARN("grid_spacing not specify. Forgot to set the request.decode = True?");
395  if (was_running)
396  ensenso_ptr_->start();
397  return true;
398  }
399  // Discard previously saved patterns
400  if (req.clear_buffer)
401  ensenso_ptr_->discardPatterns();
402  // Set the grid spacing
403  if (req.decode)
404  {
405  res.grid_spacing = ensenso_ptr_->decodePattern();
406  // Check consistency
407  if (res.grid_spacing <= 0)
408  {
409  ROS_WARN("Couldn't decode calibration pattern");
410  if (was_running)
411  ensenso_ptr_->start();
412  return true;
413  }
414  }
415  else
416  res.grid_spacing = req.grid_spacing;
417  ensenso_ptr_->setGridSpacing(res.grid_spacing);
418  // Collect pattern
419  int prev_pattern_count = ensenso_ptr_->getPatternCount();
420  res.pattern_count = ensenso_ptr_->collectPattern(req.add_to_buffer);
421  res.success = (res.pattern_count == prev_pattern_count+1);
422  if (was_running)
423  ensenso_ptr_->start();
424  return true;
425  }
426 
427  bool estimatePatternPoseCB(ensenso::EstimatePatternPose::Request& req, ensenso::EstimatePatternPose::Response &res)
428  {
429  bool was_running = ensenso_ptr_->isRunning();
430  if (was_running)
431  ensenso_ptr_->stop();
432  res.success = ensenso_ptr_->getPatternCount() > 0;
433  if (res.success)
434  {
435  Eigen::Affine3d pattern_pose;
436  res.success = ensenso_ptr_->estimatePatternPose(pattern_pose, req.average);
437  tf::poseEigenToMsg(pattern_pose, res.pose);
438  }
439  if (was_running)
440  ensenso_ptr_->start();
441  return true;
442  }
443 
444  void cloudCallback( const boost::shared_ptr<PointCloudXYZ>& cloud)
445  {
446  // Point cloud
447  if (cloud_pub_.getNumSubscribers() > 0)
448  {
449  ros::Time stamp;
450  //stamp is the same for all images/cloud
451  pcl_conversions::fromPCL(cloud->header.stamp, stamp);
452  cloud->header.frame_id = camera_frame_id_;
453  sensor_msgs::PointCloud2 cloud_msg;
454  cloud_msg.header.stamp = stamp;
455  pcl::toROSMsg(*cloud, cloud_msg);
456  cloud_pub_.publish(cloud_msg);
457  }
458  }
459 
460  void cloudRGBCallback( const boost::shared_ptr<PointCloudXYZRGBA>& cloud)
461  {
462  // Point cloud
463  if (cloud_pub_.getNumSubscribers() > 0)
464  {
465  ros::Time stamp;
466  //stamp is the same for all images/cloud
467  pcl_conversions::fromPCL(cloud->header.stamp, stamp);
468  cloud->header.frame_id = rgb_camera_frame_id_;
469  sensor_msgs::PointCloud2 cloud_msg;
470  cloud_msg.header.stamp = stamp;
471  pcl::toROSMsg(*cloud, cloud_msg);
472  cloud_pub_.publish(cloud_msg);
473  }
474  }
475 
476  void imagesCallback( const boost::shared_ptr<PairOfImages>& rawimages, const boost::shared_ptr<PairOfImages>& rectifiedimages)
477  {
478  ros::Time stamp;
479  //stamp is the same for all images
480  pcl_conversions::fromPCL(rawimages->first.header.stamp, stamp);
481  // Get cameras info
482  sensor_msgs::CameraInfo linfo, rinfo;
483  ensenso_ptr_->getCameraInfo("Left", linfo);
484  ensenso_ptr_->getCameraInfo("Right", rinfo);
485  linfo.header.stamp = stamp;
486  linfo.header.frame_id = camera_frame_id_;
487  rinfo.header.stamp = stamp;
488  rinfo.header.frame_id = camera_frame_id_;
489  // Images
490  if (l_raw_pub_.getNumSubscribers() > 0)
491  l_raw_pub_.publish(*toImageMsg(rawimages->first, stamp, camera_frame_id_), linfo, stamp);
492  if (r_raw_pub_.getNumSubscribers() > 0)
493  r_raw_pub_.publish(*toImageMsg(rawimages->second, stamp, camera_frame_id_), rinfo, stamp);
494  if (l_rectified_pub_.getNumSubscribers() > 0)
495  l_rectified_pub_.publish(toImageMsg(rectifiedimages->first, stamp, camera_frame_id_));
496  if (r_rectified_pub_.getNumSubscribers() > 0)
497  r_rectified_pub_.publish(toImageMsg(rectifiedimages->second, stamp, camera_frame_id_));
498  // Publish calibration pattern info (if any)
499  publishCalibrationPattern(stamp);
500  }
501 
502  void imagesRGBCallback( const boost::shared_ptr<PairOfImages>& rawimages, const boost::shared_ptr<PairOfImages>& rectifiedimages,
503  const boost::shared_ptr<PairOfImages>& rgbimages)
504  {
505  ros::Time stamp;
506  //stamp is the same for all images/cloud
507  pcl_conversions::fromPCL(rawimages->first.header.stamp, stamp);
508  // Get cameras info
509  sensor_msgs::CameraInfo linfo, rinfo, rgbinfo;
510  ensenso_ptr_->getCameraInfo("Left", linfo);
511  ensenso_ptr_->getCameraInfo("Right", rinfo);
512  ensenso_ptr_->getCameraInfo("RGB", rgbinfo);
513  linfo.header.stamp = stamp;
514  linfo.header.frame_id = camera_frame_id_;
515  rinfo.header.stamp = stamp;
516  rinfo.header.frame_id = camera_frame_id_;
517  rgbinfo.header.stamp = stamp;
518  rgbinfo.header.frame_id = rgb_camera_frame_id_;
519  // Images
520  if (l_raw_pub_.getNumSubscribers() > 0)
521  l_raw_pub_.publish(*toImageMsg(rawimages->first, stamp, camera_frame_id_), linfo, stamp);
522  if (r_raw_pub_.getNumSubscribers() > 0)
523  r_raw_pub_.publish(*toImageMsg(rawimages->second, stamp, camera_frame_id_), rinfo, stamp);
524  if (l_rectified_pub_.getNumSubscribers() > 0)
525  l_rectified_pub_.publish(toImageMsg(rectifiedimages->first, stamp, camera_frame_id_));
526  if (r_rectified_pub_.getNumSubscribers() > 0)
527  r_rectified_pub_.publish(toImageMsg(rectifiedimages->second, stamp, camera_frame_id_));
528  if (rgb_raw_pub_.getNumSubscribers() > 0)
529  rgb_raw_pub_.publish(*toImageMsg(rgbimages->first, stamp, rgb_camera_frame_id_), rgbinfo, stamp);
530  if (rgb_rectified_pub_.getNumSubscribers() > 0)
531  rgb_rectified_pub_.publish(toImageMsg(rgbimages->second, stamp, rgb_camera_frame_id_));
532  // Publish calibration pattern info (if any)
533  publishCalibrationPattern(stamp);
534  }
535 
536  void depthCallback( const boost::shared_ptr<pcl::PCLGenImage<float> >& depthimage)
537  {
538  if (depth_pub_.getNumSubscribers() > 0)
539  {
540  std::string frame_id = rgb_available_ ? rgb_camera_frame_id_ : camera_frame_id_;
541 
542  ros::Time stamp;
543  //stamp is the same for all images/cloud
544  pcl_conversions::fromPCL(depthimage->header.stamp, stamp);
545  sensor_msgs::CameraInfo dinfo;
546  ensenso_ptr_->getCameraInfo("Depth", dinfo);
547  dinfo.header.stamp = stamp;
548  dinfo.header.frame_id = frame_id;
549  depth_pub_.publish(*toImageMsg(*depthimage, stamp, frame_id), dinfo, stamp);
550  }
551  }
552 
553  void publishTF()
554  {
555  pcl::Transform ensenso_tf;
556  if (!ensenso_ptr_->getTFLeftToRGB(ensenso_tf))
557  {
558  return;
559  }
560  geometry_msgs::TransformStamped tf;
561  tf.header.frame_id = camera_frame_id_;
562  tf.header.stamp = ros::Time::now();
563  tf.child_frame_id = rgb_camera_frame_id_;
564  tf.transform.rotation.x = ensenso_tf.qx;
565  tf.transform.rotation.y = ensenso_tf.qy;
566  tf.transform.rotation.z = ensenso_tf.qz;
567  tf.transform.rotation.w = ensenso_tf.qw;
568  tf.transform.translation.x = ensenso_tf.tx;
569  tf.transform.translation.y = ensenso_tf.ty;
570  tf.transform.translation.z = ensenso_tf.tz;
571  tf_br_.sendTransform(tf);
572  }
573 
574  void publishCalibrationPattern(const ros::Time &now)
575  {
576  int pose_subs = pattern_pose_pub_.getNumSubscribers();
577  int raw_subs = pattern_raw_pub_.getNumSubscribers();
578  if (stream_calib_pattern_)
579  {
580  if ((pose_subs <= 0) && (raw_subs <= 0))
581  return;
582  int num_points;
583  double grid_spacing;
584  Eigen::Affine3d pattern_pose;
585  std::vector<int> grid_size;
586  std::vector<Eigen::Vector2d> left_points, right_points;
587  if ( ensenso_ptr_->getLastCalibrationPattern (grid_size, grid_spacing, left_points, right_points, pattern_pose) )
588  {
589  if (raw_subs > 0)
590  {
591  // Populate RawStereoPattern msg
592  ensenso::RawStereoPattern msg;
593  msg.header.frame_id = camera_frame_id_;
594  msg.header.stamp = now;
595  msg.grid_spacing = grid_spacing;
596  msg.grid_size = grid_size;
597  num_points = grid_size[0]*grid_size[1];
598  msg.left_points.resize(num_points);
599  msg.right_points.resize(num_points);
600  for (uint i = 0; i < left_points.size(); ++i)
601  {
602  msg.left_points[i].x = left_points[i][0];
603  msg.left_points[i].y = left_points[i][1];
604  msg.right_points[i].x = left_points[i][0];
605  msg.right_points[i].y = left_points[i][1];
606  }
607  pattern_raw_pub_.publish(msg);
608  }
609  if (pose_subs > 0)
610  {
611  // Populate PoseStamped msg
612  geometry_msgs::PoseStamped pose_msg;
613  pose_msg.header.frame_id = camera_frame_id_;
614  pose_msg.header.stamp = now;
615  tf::poseEigenToMsg(pattern_pose, pose_msg.pose);
616  pattern_pose_pub_.publish(pose_msg);
617  }
618  }
619  }
620  }
621 
622  template <typename T>
623  sensor_msgs::ImagePtr toImageMsg(pcl::PCLGenImage<T>& pcl_image, ros::Time now, std::string frame_id)
624  {
625  unsigned char *image_array = reinterpret_cast<unsigned char *>(&pcl_image.data[0]);
626  int type(CV_8UC1);
627  std::string encoding("mono8");
628  std_msgs::Header header;
629  header.stamp = now;
630  header.frame_id = frame_id;
631  //depth image
632  if (pcl_image.encoding == "CV_32FC1")
633  {
634  type = CV_32FC1;
635  encoding = "32FC1";
636  }
637  //rgb images
638  if (pcl_image.encoding == "CV_8UC3")
639  {
640  type = CV_8UC3;
641  encoding = "rgb8";
642  }
643  cv::Mat image_mat(pcl_image.height, pcl_image.width, type, image_array);
644  return cv_bridge::CvImage(header, encoding, image_mat).toImageMsg();
645  }
646 
647  void imagesSubscribeCallback ()
648  {
649  bool need_images = ((rgb_raw_pub_.getNumSubscribers() + rgb_raw_pub_.getNumSubscribers() +
650  rgb_rectified_pub_.getNumSubscribers() + l_raw_pub_.getNumSubscribers() +
651  r_raw_pub_.getNumSubscribers() + l_rectified_pub_.getNumSubscribers() +
652  r_rectified_pub_.getNumSubscribers()) > 0);
653 
654  if (enable_images_ && need_images && !is_streaming_images_)
655  {
656  if (rgb_available_ && (rgb_raw_pub_.getNumSubscribers() + rgb_rectified_pub_.getNumSubscribers()) > 0)
657  {
658  boost::function<void(
659  const boost::shared_ptr<PairOfImages>&,
660  const boost::shared_ptr<PairOfImages>&,
661  const boost::shared_ptr<PairOfImages>&)> f = boost::bind (&EnsensoDriver::imagesRGBCallback, this, _1, _2, _3);
662  image_connection_ = ensenso_ptr_->registerCallback(f);
663  }
664  else
665  {
666  boost::function<void(
667  const boost::shared_ptr<PairOfImages>&,
668  const boost::shared_ptr<PairOfImages>&)> f = boost::bind (&EnsensoDriver::imagesCallback, this, _1, _2);
669  image_connection_ = ensenso_ptr_->registerCallback(f);
670  }
671  if (!ensenso_ptr_->isRunning())
672  {
673  ensenso_ptr_->start();
674  }
675  is_streaming_images_ = true;
676  }
677  else if (( !enable_images_ || !need_images ) && is_streaming_images_)
678  {
679  image_connection_.disconnect();
680  is_streaming_images_ = false;
681  if (!is_streaming_cloud_ && !is_streaming_depth_)
682  {
683  ensenso_ptr_->stop();
684  }
685  }
686 
687  }
688 
689  void cloudSubscribeCallback ()
690  {
691  bool need_cloud = cloud_pub_.getNumSubscribers() > 0;
692  if (enable_cloud_ && need_cloud && !is_streaming_cloud_)
693  {
694  if(rgb_available_)
695  {
696  boost::function<void(
697  const boost::shared_ptr<PointCloudXYZRGBA>&)> f = boost::bind (&EnsensoDriver::cloudRGBCallback, this, _1);
698  cloud_connection_ = ensenso_ptr_->registerCallback(f);
699  }
700  else
701  {
702  boost::function<void(
703  const boost::shared_ptr<PointCloudXYZ>&)> f = boost::bind (&EnsensoDriver::cloudCallback, this, _1);
704  cloud_connection_ = ensenso_ptr_->registerCallback(f);
705  }
706  if (!ensenso_ptr_->isRunning())
707  {
708  ensenso_ptr_->start();
709  }
710  is_streaming_cloud_ = true;
711  }
712  else if ( (!enable_cloud_ || !need_cloud) && is_streaming_cloud_)
713  {
714  cloud_connection_.disconnect();
715  is_streaming_cloud_ = false;
716  if (!is_streaming_depth_ && !is_streaming_images_)
717  {
718  ensenso_ptr_->stop();
719  }
720  }
721  }
722 
723  void depthSubscribeCallback ()
724  {
725  bool need_depth = depth_pub_.getNumSubscribers() > 0;
726  if (enable_depth_ && need_depth && !is_streaming_depth_)
727  {
728  boost::function<void(
729  const boost::shared_ptr<pcl::PCLGenImage<float> >&)> f = boost::bind (&EnsensoDriver::depthCallback, this, _1);
730  depth_connection_ = ensenso_ptr_->registerCallback(f);
731  is_streaming_depth_ = true;
732  if (!ensenso_ptr_->isRunning())
733  {
734  ensenso_ptr_->start();
735  }
736  }
737  else if ((!enable_depth_ || !need_depth) && is_streaming_depth_)
738  {
739  depth_connection_.disconnect();
740  is_streaming_depth_ = false;
741  if (!is_streaming_cloud_ && !is_streaming_images_)
742  {
743  ensenso_ptr_->stop();
744  }
745  }
746  }
747 };
748 
749 
750 int main(int argc, char **argv)
751 {
752  ros::init (argc, argv, "ensenso_driver");
753  EnsensoDriver driver;
754  ros::spin();
755  ros::shutdown();
756  return 0;
757 }
EnsensoDriver::estimatePatternPoseCB
bool estimatePatternPoseCB(ensenso::EstimatePatternPose::Request &req, ensenso::EstimatePatternPose::Response &res)
Definition: ensenso_driver.cpp:427
EnsensoDriver::pattern_raw_pub_
ros::Publisher pattern_raw_pub_
Definition: ensenso_driver.cpp:62
EnsensoDriver::rgb_camera_frame_id_
std::string rgb_camera_frame_id_
Definition: ensenso_driver.cpp:76
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle
EnsensoDriver::stream_calib_pattern_
bool stream_calib_pattern_
Definition: ensenso_driver.cpp:72
EnsensoDriver::cloudCallback
void cloudCallback(const boost::shared_ptr< PointCloudXYZ > &cloud)
Definition: ensenso_driver.cpp:444
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
EnsensoDriver::rgb_raw_pub_
image_transport::CameraPublisher rgb_raw_pub_
Definition: ensenso_driver.cpp:51
f
f
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
pcl::PCLGenImage::data
std::vector< T > data
Definition: ensenso_grabber.h:24
EnsensoDriver::reconfigure_server_
dynamic_reconfigure::Server< ensenso::CameraParametersConfig > reconfigure_server_
Definition: ensenso_driver.cpp:48
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
image_transport::CameraPublisher::getNumSubscribers
uint32_t getNumSubscribers() const
EnsensoDriver::nh_
ros::NodeHandle nh_
Definition: ensenso_driver.cpp:44
image_transport.h
EnsensoDriver::tf_br_
tf2_ros::TransformBroadcaster tf_br_
Definition: ensenso_driver.cpp:77
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
EnsensoDriver::imagesCallback
void imagesCallback(const boost::shared_ptr< PairOfImages > &rawimages, const boost::shared_ptr< PairOfImages > &rectifiedimages)
Definition: ensenso_driver.cpp:476
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
image_transport::CameraPublisher
image_transport::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
transform_broadcaster.h
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
EnsensoDriver::cloudSubscribeCallback
void cloudSubscribeCallback()
Definition: ensenso_driver.cpp:689
EnsensoDriver::r_rectified_pub_
image_transport::Publisher r_rectified_pub_
Definition: ensenso_driver.cpp:57
ROS_WARN
#define ROS_WARN(...)
PointXYZRGBA
pcl::PointXYZRGBA PointXYZRGBA
Definition: ensenso_driver.cpp:34
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
EnsensoDriver::l_rectified_pub_
image_transport::Publisher l_rectified_pub_
Definition: ensenso_driver.cpp:56
EnsensoDriver::image_connection_
boost::signals2::connection image_connection_
Definition: ensenso_driver.cpp:81
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
boost::shared_ptr
EnsensoDriver::depth_connection_
boost::signals2::connection depth_connection_
Definition: ensenso_driver.cpp:82
EnsensoDriver::it_
image_transport::ImageTransport it_
Definition: ensenso_driver.cpp:50
EnsensoDriver::pattern_srv_
ros::ServiceServer pattern_srv_
Definition: ensenso_driver.cpp:45
EnsensoDriver::imagesRGBCallback
void imagesRGBCallback(const boost::shared_ptr< PairOfImages > &rawimages, const boost::shared_ptr< PairOfImages > &rectifiedimages, const boost::shared_ptr< PairOfImages > &rgbimages)
Definition: ensenso_driver.cpp:502
pcl_conversions::fromPCL
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
EnsensoDriver::cloud_pub_
ros::Publisher cloud_pub_
Definition: ensenso_driver.cpp:60
EnsensoDriver::depthCallback
void depthCallback(const boost::shared_ptr< pcl::PCLGenImage< float > > &depthimage)
Definition: ensenso_driver.cpp:536
pcl::EnsensoGrabber
Grabber for IDS-Imaging Ensenso&#39;s devices.
Definition: ensenso_grabber.h:37
cv_bridge.h
PairOfImages
std::pair< pcl::PCLGenImage< pcl::uint8_t >, pcl::PCLGenImage< pcl::uint8_t > > PairOfImages
Definition: ensenso_driver.cpp:33
EnsensoDriver::cloudRGBCallback
void cloudRGBCallback(const boost::shared_ptr< PointCloudXYZRGBA > &cloud)
Definition: ensenso_driver.cpp:460
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
cv_bridge::CvImage
EnsensoDriver::pattern_pose_pub_
ros::Publisher pattern_pose_pub_
Definition: ensenso_driver.cpp:61
ros::NodeHandle::createTimer
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
point_cloud.h
EnsensoDriver::publishCalibrationPattern
void publishCalibrationPattern(const ros::Time &now)
Definition: ensenso_driver.cpp:574
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
EnsensoDriver::ensenso_ptr_
pcl::EnsensoGrabber::Ptr ensenso_ptr_
Definition: ensenso_driver.cpp:83
EnsensoDriver::collect_srv_
ros::ServiceServer collect_srv_
Definition: ensenso_driver.cpp:46
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
EnsensoDriver::calibrateHandEyeCB
bool calibrateHandEyeCB(ensenso::CalibrateHandEye::Request &req, ensenso::CalibrateHandEye::Response &res)
Definition: ensenso_driver.cpp:178
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
EnsensoDriver::collectPatternCB
bool collectPatternCB(ensenso::CollectPattern::Request &req, ensenso::CollectPattern::Response &res)
Definition: ensenso_driver.cpp:386
EnsensoDriver::nh_private_
ros::NodeHandle nh_private_
Definition: ensenso_driver.cpp:44
EnsensoDriver::rgb_rectified_pub_
image_transport::Publisher rgb_rectified_pub_
Definition: ensenso_driver.cpp:55
EnsensoDriver::cloud_connection_
boost::signals2::connection cloud_connection_
Definition: ensenso_driver.cpp:80
main
int main(int argc, char **argv)
Definition: ensenso_driver.cpp:750
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
EnsensoDriver::depth_pub_
image_transport::CameraPublisher depth_pub_
Definition: ensenso_driver.cpp:54
tf2_ros::TransformBroadcaster
pcl::toROSMsg
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
EnsensoDriver::r_raw_pub_
image_transport::CameraPublisher r_raw_pub_
Definition: ensenso_driver.cpp:53
PointCloudXYZRGBA
pcl::PointCloud< PointXYZRGBA > PointCloudXYZRGBA
Definition: ensenso_driver.cpp:35
PointXYZ
pcl::PointXYZ PointXYZ
Definition: ensenso_driver.cpp:36
ros::Time::now
static Time now()
ros::shutdown
ROSCPP_DECL void shutdown()
EnsensoDriver::imagesSubscribeCallback
void imagesSubscribeCallback()
Definition: ensenso_driver.cpp:647
image_transport::ImageTransport
image_transport::Publisher
header
const std::string header
image_encodings.h
ensenso_grabber.h
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
PointCloudXYZ
pcl::PointCloud< PointXYZ > PointCloudXYZ
Definition: ensenso_driver.cpp:37
EnsensoDriver::camera_frame_id_
std::string camera_frame_id_
Definition: ensenso_driver.cpp:75
EnsensoDriver::cameraParametersCallback
void cameraParametersCallback(ensenso::CameraParametersConfig &config, uint32_t level)
Definition: ensenso_driver.cpp:217
EnsensoDriver::toImageMsg
sensor_msgs::ImagePtr toImageMsg(pcl::PCLGenImage< T > &pcl_image, ros::Time now, std::string frame_id)
Definition: ensenso_driver.cpp:623
encoding
std::string encoding
EnsensoDriver::calibrate_srv_
ros::ServiceServer calibrate_srv_
Definition: ensenso_driver.cpp:47
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
EnsensoDriver::tf_publisher_
ros::Timer tf_publisher_
Definition: ensenso_driver.cpp:78
eigen_msg.h
EnsensoDriver::depthSubscribeCallback
void depthSubscribeCallback()
Definition: ensenso_driver.cpp:723
ROS_DEBUG
#define ROS_DEBUG(...)
EnsensoDriver::l_raw_pub_
image_transport::CameraPublisher l_raw_pub_
Definition: ensenso_driver.cpp:52

ensenso
Author(s): Francisco Suarez Ruiz
autogenerated on Sat Feb 16 2019 03:42:20