all_cameras.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 //##################
19 //#### includes ####
20 
21 // standard includes
22 //--
23 
24 // ROS includes
25 #include <ros/ros.h>
26 #include <cv_bridge/CvBridge.h>
27 #include <image_transport/image_transport.h>
28 
29 // ROS message includes
30 #include <sensor_msgs/Image.h>
31 #include <sensor_msgs/CameraInfo.h>
32 #include <sensor_msgs/fill_image.h>
33 #include <sensor_msgs/SetCameraInfo.h>
34 
35 // external includes
36 #include <cob_camera_sensors/AbstractColorCamera.h>
37 #include <cob_camera_sensors/AbstractRangeImagingSensor.h>
38 #include <cob_vision_utils/GlobalDefines.h>
39 #include <cob_vision_utils/CameraSensorToolbox.h>
40 
41 using namespace ipa_CameraSensors;
42 
45 class CobAllCamerasNode
46 {
47 private:
48  ros::NodeHandle node_handle_;
49 
50  AbstractColorCameraPtr left_color_camera_;
51  AbstractColorCameraPtr right_color_camera_;
52  AbstractRangeImagingSensorPtr tof_camera_;
53 
54  std::string config_directory_;
55 
56  std::string left_color_camera_ns_;
57  std::string right_color_camera_ns_;
58  std::string tof_camera_ns_;
59 
60  int left_color_camera_intrinsic_id_;
61  int right_color_camera_intrinsic_id_;
62  int tof_camera_intrinsic_id_;
63 
64  ipa_CameraSensors::t_cameraType left_color_camera_intrinsic_type_;
65  ipa_CameraSensors::t_cameraType right_color_camera_intrinsic_type_;
66  ipa_CameraSensors::t_cameraType tof_camera_intrinsic_type_;
67 
68  sensor_msgs::CameraInfo left_color_camera_info_msg_;
69  sensor_msgs::CameraInfo right_color_camera_info_msg_;
70  sensor_msgs::CameraInfo tof_camera_info_msg_;
71 
72  ros::ServiceServer left_color_camera_info_service_;
73  ros::ServiceServer right_color_camera_info_service_;
74  ros::ServiceServer tof_camera_info_service_;
75 
76  cv::Mat left_color_image_8U3_;
77  cv::Mat right_color_image_8U3_;
78  cv::Mat xyz_tof_image_32F3_;
79  cv::Mat grey_tof_image_32F1_;
80 
81  image_transport::ImageTransport image_transport_;
82  image_transport::CameraPublisher xyz_tof_image_publisher_;
83  image_transport::CameraPublisher grey_tof_image_publisher_;
84  image_transport::CameraPublisher left_color_image_publisher_;
85  image_transport::CameraPublisher right_color_image_publisher_;
86 
87 public:
88  CobAllCamerasNode(const ros::NodeHandle& node_handle)
89  : node_handle_(node_handle),
90  left_color_camera_(AbstractColorCameraPtr()),
91  right_color_camera_(AbstractColorCameraPtr()),
92  tof_camera_(AbstractRangeImagingSensorPtr()),
93  left_color_image_8U3_(cv::Mat()),
94  right_color_image_8U3_(cv::Mat()),
95  xyz_tof_image_32F3_(cv::Mat()),
96  grey_tof_image_32F1_(cv::Mat()),
97  image_transport_(node_handle)
98  {
100  }
101 
102  ~CobAllCamerasNode()
103  {
104  ROS_INFO("[all_cameras] Shutting down cameras");
105  if (left_color_camera_)
106  {
107  ROS_INFO("[all_cameras] Shutting down left color camera (1)");
108  left_color_camera_->Close();
109  }
110  if (right_color_camera_)
111  {
112  ROS_INFO("[all_cameras] Shutting down right color camera (0)");
113  right_color_camera_->Close();
114  }
115  if (tof_camera_)
116  {
117  ROS_INFO("[all_cameras] Shutting down tof camera (0)");
118  tof_camera_->Close();
119  }
120  }
121 
123  bool init()
124  {
125  if (loadParameters() == false)
126  {
127  ROS_ERROR("[all_cameras] Could not read parameters from launch file");
128  return false;
129  }
130 
131  if (left_color_camera_ && (left_color_camera_->Init(config_directory_, 1) & ipa_CameraSensors::RET_FAILED))
132  {
133  ROS_WARN("[all_cameras] Initialization of left camera (1) failed");
134  left_color_camera_ = AbstractColorCameraPtr();
135  }
136 
137  if (left_color_camera_ && (left_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
138  {
139  ROS_WARN("[all_cameras] Opening left color camera (1) failed");
140  left_color_camera_ = AbstractColorCameraPtr();
141  }
142  if (left_color_camera_)
143  {
145  int camera_index = 1;
146  ipa_CameraSensors::t_cameraProperty cameraProperty;
147  cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
148  left_color_camera_->GetProperty(&cameraProperty);
149  int color_sensor_width = cameraProperty.cameraResolution.xResolution;
150  int color_sensor_height = cameraProperty.cameraResolution.yResolution;
151  cv::Size color_image_size(color_sensor_width, color_sensor_height);
152 
154  ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
155  color_sensor_toolbox->Init(config_directory_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
156 
157  cv::Mat d = color_sensor_toolbox->GetDistortionParameters(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
158  left_color_camera_info_msg_.header.stamp = ros::Time::now();
159  left_color_camera_info_msg_.header.frame_id = "head_color_camera_l_link";
160  left_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
161  left_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
162  left_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
163  left_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
164  left_color_camera_info_msg_.D[4] = 0;
165 
166  cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(left_color_camera_intrinsic_type_, left_color_camera_intrinsic_id_);
167  left_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
168  left_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
169  left_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
170  left_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
171  left_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
172  left_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
173  left_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
174  left_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
175  left_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
176 
177  left_color_camera_info_msg_.width = color_sensor_width;
178  left_color_camera_info_msg_.height = color_sensor_height;
179  }
180 
181  if (right_color_camera_ && (right_color_camera_->Init(config_directory_, 0) & ipa_CameraSensors::RET_FAILED))
182  {
183  ROS_WARN("[all_cameras] Initialization of right camera (0) failed");
184  right_color_camera_ = AbstractColorCameraPtr();
185  }
186 
187  if (right_color_camera_ && (right_color_camera_->Open() & ipa_CameraSensors::RET_FAILED))
188  {
189  ROS_WARN("[all_cameras] Opening right color camera (0) failed");
190  right_color_camera_ = AbstractColorCameraPtr();
191  }
192  if (right_color_camera_)
193  {
194  int camera_index = 0;
196  ipa_CameraSensors::t_cameraProperty cameraProperty;
197  cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
198  right_color_camera_->GetProperty(&cameraProperty);
199  int color_sensor_width = cameraProperty.cameraResolution.xResolution;
200  int color_sensor_height = cameraProperty.cameraResolution.yResolution;
201  cv::Size color_image_size(color_sensor_width, color_sensor_height);
202 
204  ipa_CameraSensors::CameraSensorToolboxPtr color_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
205  color_sensor_toolbox->Init(config_directory_, left_color_camera_->GetCameraType(), camera_index, color_image_size);
206 
207  cv::Mat d = color_sensor_toolbox->GetDistortionParameters(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
208  right_color_camera_info_msg_.header.stamp = ros::Time::now();
209  right_color_camera_info_msg_.header.frame_id = "head_color_camera_r_link";
210  right_color_camera_info_msg_.D[0] = d.at<double>(0, 0);
211  right_color_camera_info_msg_.D[1] = d.at<double>(0, 1);
212  right_color_camera_info_msg_.D[2] = d.at<double>(0, 2);
213  right_color_camera_info_msg_.D[3] = d.at<double>(0, 3);
214  right_color_camera_info_msg_.D[4] = 0;
215 
216  cv::Mat k = color_sensor_toolbox->GetIntrinsicMatrix(right_color_camera_intrinsic_type_, right_color_camera_intrinsic_id_);
217  right_color_camera_info_msg_.K[0] = k.at<double>(0, 0);
218  right_color_camera_info_msg_.K[1] = k.at<double>(0, 1);
219  right_color_camera_info_msg_.K[2] = k.at<double>(0, 2);
220  right_color_camera_info_msg_.K[3] = k.at<double>(1, 0);
221  right_color_camera_info_msg_.K[4] = k.at<double>(1, 1);
222  right_color_camera_info_msg_.K[5] = k.at<double>(1, 2);
223  right_color_camera_info_msg_.K[6] = k.at<double>(2, 0);
224  right_color_camera_info_msg_.K[7] = k.at<double>(2, 1);
225  right_color_camera_info_msg_.K[8] = k.at<double>(2, 2);
226 
227  right_color_camera_info_msg_.width = color_sensor_width;
228  right_color_camera_info_msg_.height = color_sensor_height;
229  }
230 
231  if (tof_camera_ && (tof_camera_->Init(config_directory_) & ipa_CameraSensors::RET_FAILED))
232  {
233  ROS_WARN("[all_cameras] Initialization of tof camera (0) failed");
234  tof_camera_ = AbstractRangeImagingSensorPtr();
235  }
236 
237  if (tof_camera_ && (tof_camera_->Open() & ipa_CameraSensors::RET_FAILED))
238  {
239  ROS_WARN("[all_cameras] Opening tof camera (0) failed");
240  tof_camera_ = AbstractRangeImagingSensorPtr();
241  }
242  if (tof_camera_)
243  {
244  int camera_index = 0;
246  ipa_CameraSensors::t_cameraProperty cameraProperty;
247  cameraProperty.propertyID = ipa_CameraSensors::PROP_CAMERA_RESOLUTION;
248  tof_camera_->GetProperty(&cameraProperty);
249  int range_sensor_width = cameraProperty.cameraResolution.xResolution;
250  int range_sensor_height = cameraProperty.cameraResolution.yResolution;
251  cv::Size rangeImageSize(range_sensor_width, range_sensor_height);
252 
254  ipa_CameraSensors::CameraSensorToolboxPtr tof_sensor_toolbox = ipa_CameraSensors::CreateCameraSensorToolbox();
255  tof_sensor_toolbox->Init(config_directory_, tof_camera_->GetCameraType(), camera_index, rangeImageSize);
256 
257  cv::Mat intrinsic_mat = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
258  cv::Mat distortion_map_X = tof_sensor_toolbox->GetDistortionMapX(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
259  cv::Mat distortion_map_Y = tof_sensor_toolbox->GetDistortionMapY(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
260  tof_camera_->SetIntrinsics(intrinsic_mat, distortion_map_X, distortion_map_Y);
261 
262  cv::Mat d = tof_sensor_toolbox->GetDistortionParameters(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
263  tof_camera_info_msg_.header.stamp = ros::Time::now();
264  tof_camera_info_msg_.header.frame_id = "head_tof_link";
265  tof_camera_info_msg_.D[0] = d.at<double>(0, 0);
266  tof_camera_info_msg_.D[1] = d.at<double>(0, 1);
267  tof_camera_info_msg_.D[2] = d.at<double>(0, 2);
268  tof_camera_info_msg_.D[3] = d.at<double>(0, 3);
269  tof_camera_info_msg_.D[4] = 0;
270 
271  cv::Mat k = tof_sensor_toolbox->GetIntrinsicMatrix(tof_camera_intrinsic_type_, tof_camera_intrinsic_id_);
272  tof_camera_info_msg_.K[0] = k.at<double>(0, 0);
273  tof_camera_info_msg_.K[1] = k.at<double>(0, 1);
274  tof_camera_info_msg_.K[2] = k.at<double>(0, 2);
275  tof_camera_info_msg_.K[3] = k.at<double>(1, 0);
276  tof_camera_info_msg_.K[4] = k.at<double>(1, 1);
277  tof_camera_info_msg_.K[5] = k.at<double>(1, 2);
278  tof_camera_info_msg_.K[6] = k.at<double>(2, 0);
279  tof_camera_info_msg_.K[7] = k.at<double>(2, 1);
280  tof_camera_info_msg_.K[8] = k.at<double>(2, 2);
281 
282  tof_camera_info_msg_.width = range_sensor_width;
283  tof_camera_info_msg_.height = range_sensor_height;
284  }
285 
287  if (left_color_camera_)
288  {
289  // Adapt name according to camera type
290  left_color_image_publisher_ = image_transport_.advertiseCamera(left_color_camera_ns_ + "/left/image_color", 1);
291  left_color_camera_info_service_ = node_handle_.advertiseService(left_color_camera_ns_ + "/left/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
292  }
293  if (right_color_camera_)
294  {
295  // Adapt name according to camera type
296  right_color_image_publisher_ = image_transport_.advertiseCamera(right_color_camera_ns_ + "/right/image_color", 1);
297  right_color_camera_info_service_ = node_handle_.advertiseService(right_color_camera_ns_ + "/right/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
298  }
299  if (tof_camera_)
300  {
301  grey_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_grey", 1);
302  xyz_tof_image_publisher_ = image_transport_.advertiseCamera(tof_camera_ns_ + "/image_xyz", 1);
303  tof_camera_info_service_ = node_handle_.advertiseService(tof_camera_ns_ + "/set_camera_info", &CobAllCamerasNode::setCameraInfo, this);
304  }
305 
306  return true;
307  }
308 
313  bool setCameraInfo(sensor_msgs::SetCameraInfo::Request& req,
314  sensor_msgs::SetCameraInfo::Response& rsp)
315  {
317  //camera_info_message_ = req.camera_info;
318 
319  rsp.success = false;
320  rsp.status_message = "[all_cameras] Setting camera parameters through ROS not implemented";
321 
322  return true;
323  }
324 
326  void spin()
327  {
328  // Set maximal spinning rate
329  ros::Rate rate(30);
330  while(node_handle_.ok())
331  {
332  // ROS images
333  sensor_msgs::Image right_color_image_msg;
334  sensor_msgs::Image left_color_image_msg;
335  sensor_msgs::Image xyz_tof_image_msg;
336  sensor_msgs::Image grey_tof_image_msg;
337 
338  // ROS camera information messages
339  sensor_msgs::CameraInfo right_color_image_info;
340  sensor_msgs::CameraInfo left_color_image_info;
341  sensor_msgs::CameraInfo tof_image_info;
342 
343  ros::Time now = ros::Time::now();
344 
345  // Acquire right color image
346  if (right_color_camera_)
347  {
348  //ROS_INFO("[all_cameras] RIGHT");
350  if (right_color_camera_->GetColorImage(&right_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
351  {
352  ROS_ERROR("[all_cameras] Right color image acquisition failed");
353  break;
354  }
355 
356  try
357  {
358  IplImage img = right_color_image_8U3_;
359  right_color_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
360  }
361  catch (sensor_msgs::CvBridgeException error)
362  {
363  ROS_ERROR("[all_cameras] Could not convert right IplImage to ROS message");
364  break;
365  }
366  right_color_image_msg.header.stamp = now;
367  right_color_image_msg.encoding = "bgr8";
368  right_color_image_msg.header.frame_id = "head_color_camera_r_link";
369 
370  right_color_image_info = right_color_camera_info_msg_;
371  right_color_image_info.width = right_color_image_8U3_.cols;
372  right_color_image_info.height = right_color_image_8U3_.rows;
373  right_color_image_info.header.stamp = now;
374  right_color_image_info.header.frame_id = "head_color_camera_r_link";
375 
376  right_color_image_publisher_.publish(right_color_image_msg, right_color_image_info);
377  }
378 
379  // Acquire left color image
380  if (left_color_camera_)
381  {
382  //ROS_INFO("[all_cameras] LEFT");
383 
385  if (left_color_camera_->GetColorImage(&left_color_image_8U3_, false) & ipa_Utils::RET_FAILED)
386  {
387  ROS_ERROR("[all_cameras] Left color image acquisition failed");
388  break;
389  }
390 
391  try
392  {
393  IplImage img = left_color_image_8U3_;
394  left_color_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
395  }
396  catch (sensor_msgs::CvBridgeException error)
397  {
398  ROS_ERROR("[all_cameras] Could not convert left IplImage to ROS message");
399  break;
400  }
401  left_color_image_msg.header.stamp = now;
402  left_color_image_msg.encoding = "bgr8";
403  left_color_image_msg.header.frame_id = "head_color_camera_l_link";
404 
405  left_color_image_info = left_color_camera_info_msg_;
406  left_color_image_info.width = left_color_image_8U3_.cols;
407  left_color_image_info.height = left_color_image_8U3_.rows;
408  left_color_image_info.header.stamp = now;
409  left_color_image_info.header.frame_id = "head_color_camera_l_link";
410 
411  left_color_image_publisher_.publish(left_color_image_msg, left_color_image_info);
412  }
413 
414  // Acquire image from tof camera
415  if (tof_camera_)
416  {
417  //ROS_INFO("[all_cameras] TOF");
418  if(tof_camera_->AcquireImages(0, &grey_tof_image_32F1_, &xyz_tof_image_32F3_, false, false, ipa_CameraSensors::INTENSITY_32F1) & ipa_Utils::RET_FAILED)
419  {
420  ROS_ERROR("[all_cameras] Tof image acquisition failed");
421  tof_camera_->Close();
422  tof_camera_ = AbstractRangeImagingSensorPtr();
423  break;
424  }
425 
426  try
427  {
428  IplImage grey_img = grey_tof_image_32F1_;
429  IplImage xyz_img = xyz_tof_image_32F3_;
430  xyz_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&xyz_img, "passthrough"));
431  grey_tof_image_msg = *(sensor_msgs::CvBridge::cvToImgMsg(&grey_img, "passthrough"));
432  }
433  catch (sensor_msgs::CvBridgeException error)
434  {
435  ROS_ERROR("[all_cameras] Could not convert tof IplImage to ROS message");
436  break;
437  }
438 
439  xyz_tof_image_msg.header.stamp = now;
440  xyz_tof_image_msg.header.frame_id = "head_tof_link";
441  grey_tof_image_msg.header.stamp = now;
442  grey_tof_image_msg.header.frame_id = "head_tof_link";
443 
444  tof_image_info = tof_camera_info_msg_;
445  tof_image_info.width = grey_tof_image_32F1_.cols;
446  tof_image_info.height = grey_tof_image_32F1_.rows;
447  tof_image_info.header.stamp = now;
448  tof_image_info.header.frame_id = "head_tof_link";
449 
450  grey_tof_image_publisher_.publish(grey_tof_image_msg, tof_image_info);
451  xyz_tof_image_publisher_.publish(xyz_tof_image_msg, tof_image_info);
452  }
453 
454  ros::spinOnce();
455  rate.sleep();
456 
457  } // END while-loop
458  }
459 
460  bool loadParameters()
461  {
462  std::string tmp_string = "NULL";
463 
465  if (node_handle_.getParam("all_cameras/configuration_files", config_directory_) == false)
466  {
467  ROS_ERROR("Path to xml configuration file not specified");
468  return false;
469  }
470 
471  ROS_INFO("Configuration directory: %s", config_directory_.c_str());
472 
473  // Color camera type
474  if (node_handle_.getParam("all_cameras/color_camera_type", tmp_string) == false)
475  {
476  ROS_ERROR("[all_cameras] Color camera type not specified");
477  return false;
478  }
479  if (tmp_string == "CAM_AVTPIKE")
480  {
481  right_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
482  left_color_camera_ = ipa_CameraSensors::CreateColorCamera_AVTPikeCam();
483 
484  left_color_camera_ns_ = "pike_145C";
485  right_color_camera_ns_ = "pike_145C";
486  }
487  else if (tmp_string == "CAM_VIRTUAL")
488  {
489  right_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
490  left_color_camera_ = ipa_CameraSensors::CreateColorCamera_VirtualCam();
491  left_color_camera_ns_ = "pike_145C";
492  right_color_camera_ns_ = "pike_145C";
493  }
494  else if (tmp_string == "CAM_PROSILICA") ROS_ERROR("[all_cameras] Color camera type not CAM_PROSILICA not yet implemented");
495  else
496  {
497  std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_AVTPIKE' or 'CAM_PROSILICA'";
498  ROS_ERROR("%s", str.c_str());
499  return false;
500  }
501 
502  ROS_INFO("Color camera type: %s", tmp_string.c_str());
503 
504  // Tof camera type
505  if (node_handle_.getParam("all_cameras/tof_camera_type", tmp_string) == false)
506  {
507  ROS_ERROR("[all_cameras] tof camera type not specified");
508  return false;
509  }
510  if (tmp_string == "CAM_SWISSRANGER")
511  {
512  tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_Swissranger();
513  tof_camera_ns_ = "sr4000";
514  }
515  else if (tmp_string == "CAM_VIRTUAL")
516  {
517  tof_camera_ = ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam();
518  tof_camera_ns_ = "sr4000";
519  }
520  else
521  {
522  std::string str = "[all_cameras] Camera type '" + tmp_string + "' unknown, try 'CAM_SWISSRANGER'";
523  ROS_ERROR("%s", str.c_str());
524  }
525 
526  ROS_INFO("Tof camera type: %s", tmp_string.c_str());
527 
528  // There are several intrinsic matrices, optimized to different cameras
529  // Here, we specified the desired intrinsic matrix for each camera
530  if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_type", tmp_string) == false)
531  {
532  ROS_ERROR("[all_cameras] Intrinsic camera type for left color camera not specified");
533  return false;
534  }
535  if (tmp_string == "CAM_AVTPIKE")
536  {
537  left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
538  }
539  else if (tmp_string == "CAM_PROSILICA")
540  {
541  left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
542  }
543  else if (tmp_string == "CAM_SWISSRANGER")
544  {
545  left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
546  }
547  else if (tmp_string == "CAM_VIRTUALRANGE")
548  {
549  left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
550  }
551  else if (tmp_string == "CAM_VIRTUALCOLOR")
552  {
553  left_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
554  }
555  else
556  {
557  std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics of left camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
558  ROS_ERROR("%s", str.c_str());
559  return false;
560  }
561  if (node_handle_.getParam("all_cameras/left_color_camera_intrinsic_id", left_color_camera_intrinsic_id_) == false)
562  {
563  ROS_ERROR("[all_cameras] Intrinsic camera id for left color camera not specified");
564  return false;
565  }
566 
567 
568  ROS_INFO("Intrinsic for left color camera: %s_%d", tmp_string.c_str(), left_color_camera_intrinsic_id_);
569 
570  // There are several intrinsic matrices, optimized to different cameras
571  // Here, we specified the desired intrinsic matrix for each camera
572  if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_type", tmp_string) == false)
573  {
574  ROS_ERROR("[all_cameras] Intrinsic camera type for right color camera not specified");
575  return false;
576  }
577  if (tmp_string == "CAM_AVTPIKE")
578  {
579  right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
580  }
581  else if (tmp_string == "CAM_PROSILICA")
582  {
583  right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
584  }
585  else if (tmp_string == "CAM_SWISSRANGER")
586  {
587  right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
588  }
589  else if (tmp_string == "CAM_VIRTUALRANGE")
590  {
591  right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
592  }
593  else if (tmp_string == "CAM_VIRTUALCOLOR")
594  {
595  right_color_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
596  }
597  else
598  {
599  std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics of right camera unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
600  ROS_ERROR("%s", str.c_str());
601  return false;
602  }
603  if (node_handle_.getParam("all_cameras/right_color_camera_intrinsic_id", right_color_camera_intrinsic_id_) == false)
604  {
605  ROS_ERROR("[all_cameras] Intrinsic camera id for right color camera not specified");
606  return false;
607  }
608 
609  ROS_INFO("Intrinsic for right color camera: %s_%d", tmp_string.c_str(), right_color_camera_intrinsic_id_);
610 
611  // There are several intrinsic matrices, optimized to different cameras
612  // Here, we specified the desired intrinsic matrix for each camera
613  if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_type", tmp_string) == false)
614  {
615  ROS_ERROR("[all_cameras] Intrinsic camera type for tof camera not specified");
616  return false;
617  }
618  if (tmp_string == "CAM_AVTPIKE")
619  {
620  tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_AVTPIKE;
621  }
622  else if (tmp_string == "CAM_PROSILICA")
623  {
624  tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_PROSILICA;
625  }
626  else if (tmp_string == "CAM_SWISSRANGER")
627  {
628  tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_SWISSRANGER;
629  }
630  else if (tmp_string == "CAM_VIRTUALRANGE")
631  {
632  tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALRANGE;
633  }
634  else if (tmp_string == "CAM_VIRTUALCOLOR")
635  {
636  tof_camera_intrinsic_type_ = ipa_CameraSensors::CAM_VIRTUALCOLOR;
637  }
638  else
639  {
640  std::string str = "[all_cameras] Camera type '" + tmp_string + "' for intrinsics unknown, try 'CAM_AVTPIKE','CAM_PROSILICA' or 'CAM_SWISSRANGER'";
641  ROS_ERROR("%s", str.c_str());
642  return false;
643  }
644  if (node_handle_.getParam("all_cameras/tof_camera_intrinsic_id", tof_camera_intrinsic_id_) == false)
645  {
646  ROS_ERROR("[all_cameras] Intrinsic camera id for tof camera not specified");
647  return false;
648  }
649 
650  ROS_INFO("Intrinsic for tof camera: %s_%d", tmp_string.c_str(), tof_camera_intrinsic_id_);
651 
652  return true;
653  }
654 };
655 
656 //#######################
657 //#### main programm ####
658 int main(int argc, char** argv)
659 {
661  ros::init(argc, argv, "color_camera");
662 
664  ros::NodeHandle nh;
665 
667  CobAllCamerasNode camera_node(nh);
668 
670  if (camera_node.init() == false)
671  {
672  ROS_ERROR("[all_cameras] Node initialization FAILED. Terminating");
673  return 0;
674  }
675  else
676  {
677  ROS_INFO("[all_cameras] Node initialization OK. Enter spinning");
678  }
679 
680  camera_node.spin();
681 
682  return 0;
683 }
CobAllCamerasNode::xyz_tof_image_32F3_
cv::Mat xyz_tof_image_32F3_
Definition: all_cameras.cpp:78
d
d
ipa_CameraSensors
ipa_CameraSensors::CAM_SWISSRANGER
CAM_SWISSRANGER
CobAllCamerasNode::image_transport_
image_transport::ImageTransport image_transport_
OpenCV image holding the amplitude values of the point cloud.
Definition: all_cameras.cpp:81
ipa_CameraSensors::RET_FAILED
RET_FAILED
CobAllCamerasNode::node_handle_
ros::NodeHandle node_handle_
Definition: all_cameras.cpp:48
image_transport::CameraPublisher::publish
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
ipa_Utils::RET_FAILED
RET_FAILED
ros::NodeHandle
CobAllCamerasNode::spin
void spin()
Callback function for image requests on topic &#39;request_image&#39;.
Definition: all_cameras.cpp:326
ipa_CameraSensors::PROP_CAMERA_RESOLUTION
PROP_CAMERA_RESOLUTION
ipa_CameraSensors::t_cameraProperty::cameraResolution
t_cameraResolution cameraResolution
CobAllCamerasNode::right_color_image_publisher_
image_transport::CameraPublisher right_color_image_publisher_
Publishes grey image data.
Definition: all_cameras.cpp:85
CobAllCamerasNode::tof_camera_ns_
std::string tof_camera_ns_
Namespace name of left color camera.
Definition: all_cameras.cpp:58
ipa_CameraSensors::AbstractColorCameraPtr
boost::shared_ptr< AbstractColorCamera > AbstractColorCameraPtr
Definition: AbstractColorCamera.h:43
CobAllCamerasNode::setCameraInfo
bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp)
Definition: all_cameras.cpp:313
image_transport.h
CobAllCamerasNode::tof_camera_info_service_
ros::ServiceServer tof_camera_info_service_
Definition: all_cameras.cpp:74
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
image_transport::CameraPublisher
CobAllCamerasNode::right_color_camera_ns_
std::string right_color_camera_ns_
Namespace name of left color camera.
Definition: all_cameras.cpp:57
CobAllCamerasNode::tof_camera_
AbstractRangeImagingSensorPtr tof_camera_
Time-of-flight camera instance.
Definition: all_cameras.cpp:52
ipa_CameraSensors::CreateRangeImagingSensor_VirtualCam
__DLL_LIBCAMERASENSORS__ AbstractRangeImagingSensorPtr CreateRangeImagingSensor_VirtualCam()
Definition: VirtualRangeCam.cpp:35
CobAllCamerasNode::tof_camera_intrinsic_type_
ipa_CameraSensors::t_cameraType tof_camera_intrinsic_type_
Instrinsic matrix type of tof camera.
Definition: all_cameras.cpp:66
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ipa_CameraSensors::CreateCameraSensorToolbox
__DLL_LIBCAMERASENSORS__ CameraSensorToolboxPtr CreateCameraSensorToolbox()
fill_image.h
ROS_WARN
#define ROS_WARN(...)
CobAllCamerasNode::tof_camera_intrinsic_id_
int tof_camera_intrinsic_id_
Instrinsic matrix id of tof camera.
Definition: all_cameras.cpp:62
CobAllCamerasNode::left_color_camera_info_service_
ros::ServiceServer left_color_camera_info_service_
Definition: all_cameras.cpp:72
CobAllCamerasNode::left_color_image_publisher_
image_transport::CameraPublisher left_color_image_publisher_
Publishes grey image data.
Definition: all_cameras.cpp:84
boost::shared_ptr
CobAllCamerasNode::right_color_camera_info_msg_
sensor_msgs::CameraInfo right_color_camera_info_msg_
ROS camera information message (e.g. holding intrinsic parameters)
Definition: all_cameras.cpp:69
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
CobAllCamerasNode::right_color_camera_intrinsic_type_
ipa_CameraSensors::t_cameraType right_color_camera_intrinsic_type_
Instrinsic matrix type of right color camera.
Definition: all_cameras.cpp:65
ipa_CameraSensors::CAM_VIRTUALRANGE
CAM_VIRTUALRANGE
CobAllCamerasNode::right_color_image_8U3_
cv::Mat right_color_image_8U3_
color image of right camera
Definition: all_cameras.cpp:77
ROS_INFO
#define ROS_INFO(...)
CobAllCamerasNode::right_color_camera_
AbstractColorCameraPtr right_color_camera_
Color camera instance.
Definition: all_cameras.cpp:51
CobAllCamerasNode::left_color_camera_ns_
std::string left_color_camera_ns_
Namespace name of left color camera.
Definition: all_cameras.cpp:56
CobAllCamerasNode::grey_tof_image_32F1_
cv::Mat grey_tof_image_32F1_
OpenCV image holding the point cloud from tof sensor.
Definition: all_cameras.cpp:79
CobAllCamerasNode::left_color_camera_intrinsic_id_
int left_color_camera_intrinsic_id_
Instrinsic matrix id of left color camera.
Definition: all_cameras.cpp:60
ipa_CameraSensors::CreateColorCamera_VirtualCam
__DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr CreateColorCamera_VirtualCam()
Definition: VirtualColorCam.cpp:34
ipa_CameraSensors::AbstractRangeImagingSensorPtr
boost::shared_ptr< AbstractRangeImagingSensor > AbstractRangeImagingSensorPtr
Definition: AbstractRangeImagingSensor.h:47
ipa_CameraSensors::t_cameraProperty
CobAllCamerasNode::config_directory_
std::string config_directory_
Directory of the configuration files.
Definition: all_cameras.cpp:54
CobAllCamerasNode::tof_camera_info_msg_
sensor_msgs::CameraInfo tof_camera_info_msg_
ROS camera information message (e.g. holding intrinsic parameters)
Definition: all_cameras.cpp:70
CobAllCamerasNode::left_color_image_8U3_
cv::Mat left_color_image_8U3_
color image of left camera
Definition: all_cameras.cpp:76
ipa_CameraSensors::t_cameraResolution::yResolution
int yResolution
CobAllCamerasNode::grey_tof_image_publisher_
image_transport::CameraPublisher grey_tof_image_publisher_
Publishes grey image data.
Definition: all_cameras.cpp:83
CobAllCamerasNode::init
bool init()
Opens the camera sensor.
Definition: all_cameras.cpp:123
ipa_CameraSensors::CAM_VIRTUALCOLOR
CAM_VIRTUALCOLOR
ros::Rate::sleep
bool sleep()
CobAllCamerasNode::xyz_tof_image_publisher_
image_transport::CameraPublisher xyz_tof_image_publisher_
Publishes xyz image data.
Definition: all_cameras.cpp:82
ipa_CameraSensors::CAM_AVTPIKE
CAM_AVTPIKE
CobAllCamerasNode::right_color_camera_intrinsic_id_
int right_color_camera_intrinsic_id_
Instrinsic matrix id of right color camera.
Definition: all_cameras.cpp:61
ipa_CameraSensors::t_cameraProperty::propertyID
t_cameraPropertyID propertyID
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
CobAllCamerasNode::left_color_camera_intrinsic_type_
ipa_CameraSensors::t_cameraType left_color_camera_intrinsic_type_
Instrinsic matrix type of left color camera.
Definition: all_cameras.cpp:64
CobAllCamerasNode::left_color_camera_
AbstractColorCameraPtr left_color_camera_
Color camera instance.
Definition: all_cameras.cpp:50
ipa_CameraSensors::t_cameraResolution::xResolution
int xResolution
ros::Time::now
static Time now()
image_transport::ImageTransport
ipa_CameraSensors::CreateRangeImagingSensor_Swissranger
__DLL_LIBCAMERASENSORS__ AbstractRangeImagingSensorPtr CreateRangeImagingSensor_Swissranger()
Definition: Swissranger.cpp:31
ros::NodeHandle::ok
bool ok() const
main
int main(int argc, char **argv)
Definition: all_cameras.cpp:658
CobAllCamerasNode::right_color_camera_info_service_
ros::ServiceServer right_color_camera_info_service_
Definition: all_cameras.cpp:73
ros::spinOnce
ROSCPP_DECL void spinOnce()
CobAllCamerasNode::CobAllCamerasNode
CobAllCamerasNode(const ros::NodeHandle &node_handle)
Definition: all_cameras.cpp:88
ipa_CameraSensors::CAM_PROSILICA
CAM_PROSILICA
ROS_ERROR
#define ROS_ERROR(...)
ipa_CameraSensors::CreateColorCamera_AVTPikeCam
__DLL_LIBCAMERASENSORS__ AbstractColorCameraPtr CreateColorCamera_AVTPikeCam()
Definition: AVTPikeCam.cpp:36
ipa_CameraSensors::t_cameraType
t_cameraType
CobAllCamerasNode::left_color_camera_info_msg_
sensor_msgs::CameraInfo left_color_camera_info_msg_
ROS camera information message (e.g. holding intrinsic parameters)
Definition: all_cameras.cpp:68
set_camera_info.req
req
Definition: set_camera_info.py:24

cob_camera_sensors
Author(s): Jan Fischer , Richard Bormann
autogenerated on Thu Mar 19 2020 03:23:05