00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 #include <ros/ros.h>
00061 #include <cv_bridge/CvBridge.h>
00062 #include <image_transport/image_transport.h>
00063 #include <image_transport/subscriber_filter.h>
00064 #include <message_filters/subscriber.h>
00065 #include <message_filters/time_synchronizer.h>
00066 #ifdef __ROS_1_1__
00067 #include <message_filters/sync_policies/approximate_time.h>
00068 #include <message_filters/synchronizer.h>
00069 #endif
00070
00071
00072 #include <sensor_msgs/Image.h>
00073 #include <sensor_msgs/CameraInfo.h>
00074 #include <sensor_msgs/fill_image.h>
00075
00076 #include <cob_camera_sensors/AcquireCalibrationImages.h>
00077
00078
00079 #include <cob_vision_utils/VisionUtils.h>
00080
00081 #include <opencv/highgui.h>
00082 #include <boost/thread/mutex.hpp>
00083
00084 using namespace message_filters;
00085
00086 #ifdef __ROS_1_1__
00087 typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> ThreeImageSyncPolicy;
00088 typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> TwoImageSyncPolicy;
00089 #endif
00090
00096 class AllCameraViewer
00097 {
00098 private:
00099 ros::NodeHandle node_handle_;
00100
00101 image_transport::ImageTransport image_transport_;
00102
00103
00104 image_transport::SubscriberFilter left_color_camera_image_sub_;
00105 image_transport::SubscriberFilter right_color_camera_image_sub_;
00106 image_transport::SubscriberFilter tof_camera_grey_image_sub_;
00107 message_filters::Subscriber<sensor_msgs::CameraInfo> left_camera_info_sub_;
00108 message_filters::Subscriber<sensor_msgs::CameraInfo> right_camera_info_sub_;
00109 image_transport::Subscriber sub;
00110
00111 #ifdef __ROS_1_1__
00112 message_filters::Synchronizer<ThreeImageSyncPolicy> shared_sub_sync_;
00113 message_filters::Synchronizer<TwoImageSyncPolicy> stereo_sub_sync_;
00114 message_filters::Synchronizer<ThreeImageSyncPolicy> all_sub_sync_;
00115 #else
00116 message_filters::TimeSynchronizer<sensor_msgs::Image,
00117 sensor_msgs::Image, sensor_msgs::Image> shared_sub_sync_;
00118 message_filters::TimeSynchronizer<sensor_msgs::Image,
00119 sensor_msgs::Image> stereo_sub_sync_;
00120 message_filters::TimeSynchronizer<sensor_msgs::Image,
00121 sensor_msgs::Image, sensor_msgs::Image> all_sub_sync_;
00122 #endif
00123
00124 int sub_counter_;
00125 int tof_image_counter_;
00126
00127 sensor_msgs::Image color_image_msg_;
00128 sensor_msgs::Image xyz_image_msg_;
00129 sensor_msgs::Image confidence_mask_msg_;
00130
00131 IplImage* right_color_image_8U3_;
00132 IplImage* left_color_image_8U3_;
00133 IplImage* xyz_image_32F3_;
00134 IplImage* grey_image_32F1_;
00135
00136 cv::Mat right_color_mat_8U3_;
00137 cv::Mat left_color_mat_8U3_;
00138 cv::Mat xyz_mat_32F3_;
00139 cv::Mat grey_mat_32F1_;
00140 cv::Mat grey_mat_8U1_;
00141 std::vector<cv::Mat> vec_grey_mat_32F1_;
00142
00143 int image_counter_;
00144
00145 sensor_msgs::CvBridge cv_bridge_0_;
00146 sensor_msgs::CvBridge cv_bridge_1_;
00147 sensor_msgs::CvBridge cv_bridge_2_;
00148
00149 ros::ServiceServer save_camera_images_service_;
00150
00151 boost::mutex m_ServiceMutex;
00152
00153 std::string absolute_output_directory_path_;
00154
00155 bool use_tof_camera_;
00156 bool use_left_color_camera_;
00157 bool use_right_color_camera_;
00158 public:
00160 AllCameraViewer(const ros::NodeHandle& node_handle)
00161 : node_handle_(node_handle),
00162 image_transport_(node_handle),
00163 #ifdef __ROS_1_1__
00164 shared_sub_sync_(ThreeImageSyncPolicy(3)),
00165 stereo_sub_sync_(TwoImageSyncPolicy(3)),
00166 all_sub_sync_(ThreeImageSyncPolicy(3)),
00167 #else
00168 shared_sub_sync_(3),
00169 stereo_sub_sync_(3),
00170 all_sub_sync_(3),
00171 #endif
00172 sub_counter_(0),
00173 right_color_image_8U3_(0),
00174 left_color_image_8U3_(0),
00175 xyz_image_32F3_(0),
00176 grey_image_32F1_(0)
00177 {
00178 use_tof_camera_ = true;
00179 use_left_color_camera_ = true;
00180 use_right_color_camera_ = true;
00181 image_counter_ = 0;
00182 tof_image_counter_ = 0;
00183 }
00184
00186 ~AllCameraViewer()
00187 {
00188 }
00189
00194 bool init()
00195 {
00196 if (loadParameters() == false) return false;
00197
00198 image_transport::SubscriberStatusCallback imgConnect = boost::bind(&AllCameraViewer::connectCallback, this);
00199 image_transport::SubscriberStatusCallback imgDisconnect = boost::bind(&AllCameraViewer::disconnectCallback, this);
00200
00201 save_camera_images_service_ = node_handle_.advertiseService("save_camera_images", &AllCameraViewer::saveCameraImagesServiceCallback, this);
00202
00203
00204
00205
00206
00207
00208 if(use_right_color_camera_ && use_tof_camera_ && !use_left_color_camera_)
00209 {
00210 ROS_INFO("[all_camera_viewer] Setting up subscribers for right color and tof camera");
00211 shared_sub_sync_.connectInput(right_color_camera_image_sub_, tof_camera_grey_image_sub_);
00212 shared_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::sharedModeSrvCallback, this, _1, _2));
00213 }
00214 else if(use_right_color_camera_ && !use_tof_camera_ && use_left_color_camera_)
00215 {
00216 ROS_INFO("[all_camera_viewer] Setting up subscribers left and right color camera");
00217 stereo_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_);
00218 stereo_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::stereoModeSrvCallback, this, _1, _2));
00219 }
00220 else if(use_right_color_camera_ && use_tof_camera_ && use_left_color_camera_)
00221 {
00222 ROS_INFO("[all_camera_viewer] Setting up subscribers for left color, right color and tof camera");
00223 all_sub_sync_.connectInput(left_color_camera_image_sub_, right_color_camera_image_sub_, tof_camera_grey_image_sub_);
00224 all_sub_sync_.registerCallback(boost::bind(&AllCameraViewer::allModeSrvCallback, this, _1, _2, _3));
00225 }
00226 else
00227 {
00228 ROS_ERROR("[all_camera_viewer] Specified camera configuration not available");
00229 return false;
00230 }
00231
00232
00233 connectCallback();
00234
00235 ROS_INFO("[all_camera_viewer] Initializing [OK]");
00236 return true;
00237 }
00238
00240 void connectCallback()
00241 {
00242 if (sub_counter_ == 0)
00243 {
00244 sub_counter_++;
00245 ROS_DEBUG("[all_camera_viewer] Subscribing to camera topics");
00246
00247 if (use_right_color_camera_)
00248 {
00249 right_color_camera_image_sub_.subscribe(image_transport_, "right/image_color", 1);
00250 right_camera_info_sub_.subscribe(node_handle_, "right/camera_info", 1);
00251 }
00252 if (use_left_color_camera_)
00253 {
00254 left_color_camera_image_sub_.subscribe(image_transport_, "left/image_color", 1);
00255 left_camera_info_sub_.subscribe(node_handle_, "left/camera_info", 1);
00256 }
00257 if (use_tof_camera_)
00258 {
00259 tof_camera_grey_image_sub_.subscribe(image_transport_, "image_grey", 1);
00260 }
00261 }
00262 }
00263
00265 void disconnectCallback()
00266 {
00267 sub_counter_--;
00268 if (sub_counter_ == 0)
00269 {
00270 ROS_DEBUG("[all_camera_viewer] Unsubscribing from camera topics");
00271
00272 if (use_right_color_camera_)
00273 {
00274 right_color_camera_image_sub_.unsubscribe();
00275 right_camera_info_sub_.unsubscribe();
00276 }
00277 if (use_left_color_camera_)
00278 {
00279 left_color_camera_image_sub_.unsubscribe();
00280 left_camera_info_sub_.unsubscribe();
00281 }
00282 if (use_tof_camera_)
00283 {
00284 tof_camera_grey_image_sub_.unsubscribe();
00285 }
00286 }
00287 }
00288
00289 void allModeSrvCallback(const sensor_msgs::ImageConstPtr& left_camera_data,
00290 const sensor_msgs::ImageConstPtr& right_camera_data,
00291 const sensor_msgs::ImageConstPtr& tof_camera_grey_data)
00292 {
00293 ROS_INFO("[all_camera_viewer] allModeSrvCallback");
00294 boost::mutex::scoped_lock lock(m_ServiceMutex);
00295
00296 try
00297 {
00298 right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "bgr8");
00299 left_color_image_8U3_ = cv_bridge_1_.imgMsgToCv(left_camera_data, "bgr8");
00300 grey_image_32F1_ = cv_bridge_2_.imgMsgToCv(tof_camera_grey_data, "passthrough");
00301
00302 cv::Mat tmp = right_color_image_8U3_;
00303 right_color_mat_8U3_ = tmp.clone();
00304
00305 tmp = left_color_image_8U3_;
00306 left_color_mat_8U3_ = tmp.clone();
00307
00308 tmp = grey_image_32F1_;
00309 grey_mat_32F1_ = tmp.clone();
00310 }
00311 catch (sensor_msgs::CvBridgeException& e)
00312 {
00313 ROS_ERROR("[all_camera_viewer] Could not convert stereo images with cv_bridge.");
00314 }
00315
00316
00317
00318 updateTofGreyscaleData();
00319
00320 cv::imshow("TOF grey data", grey_mat_8U1_);
00321
00322 cv::Mat right_color_8U3;
00323 cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
00324 cv::imshow("Right color data", right_color_8U3);
00325
00326 cv::Mat left_color_8U3;
00327 cv::resize(left_color_mat_8U3_, left_color_8U3, cv::Size(), 0.5, 0.5);
00328 cv::imshow("Left color data", left_color_8U3);
00329 cv::waitKey(50);
00330
00331 ROS_INFO("[all_camera_viewer] allModeSrvCallback [OK]");
00332 }
00333
00336 void sharedModeSrvCallback(const sensor_msgs::ImageConstPtr& right_camera_data,
00337 const sensor_msgs::ImageConstPtr& tof_camera_grey_data)
00338 {
00339 boost::mutex::scoped_lock lock(m_ServiceMutex);
00340 ROS_INFO("[all_camera_viewer] sharedModeSrvCallback");
00341
00342 try
00343 {
00344 right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "passthrough");
00345 grey_image_32F1_ = cv_bridge_2_.imgMsgToCv(tof_camera_grey_data, "passthrough");
00346
00347 cv::Mat tmp = right_color_image_8U3_;
00348 right_color_mat_8U3_ = tmp.clone();
00349
00350 tmp = grey_image_32F1_;
00351 grey_mat_32F1_ = tmp.clone();
00352 }
00353 catch (sensor_msgs::CvBridgeException& e)
00354 {
00355 ROS_ERROR("[all_camera_viewer] Could not convert images with cv_bridge.");
00356 }
00357
00358
00359
00360 updateTofGreyscaleData();
00361
00362 cv::imshow("TOF grey data", grey_mat_8U1_);
00363
00364 cv::Mat right_color_8U3;
00365 cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
00366 cv::imshow("Right color data", right_color_8U3);
00367 cv::waitKey(1000);
00368 }
00369
00373 void updateTofGreyscaleData()
00374 {
00375 cv::Mat filtered_grey_mat_32F1 = cv::Mat::zeros(grey_mat_32F1_.rows, grey_mat_32F1_.cols, CV_8UC1);
00376
00377
00378 if (vec_grey_mat_32F1_.size() <= (unsigned int) tof_image_counter_)
00379 vec_grey_mat_32F1_.push_back(grey_mat_32F1_);
00380 else
00381 vec_grey_mat_32F1_[tof_image_counter_] = grey_mat_32F1_;
00382
00383 tof_image_counter_ = (++tof_image_counter_)%5;
00384
00385
00386 for (unsigned int i=0; i<vec_grey_mat_32F1_.size(); i++)
00387 filtered_grey_mat_32F1 += vec_grey_mat_32F1_[i];
00388 filtered_grey_mat_32F1 * (1/vec_grey_mat_32F1_.size());
00389
00390
00391 cv::Mat temp0_grey_mat_8U1;
00392 cv::Mat temp1_grey_mat_8U1;
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407 grey_mat_8U1_ = filtered_grey_mat_32F1;
00408
00409 }
00410
00413 void stereoModeSrvCallback(const sensor_msgs::ImageConstPtr& left_camera_data,
00414 const sensor_msgs::ImageConstPtr& right_camera_data)
00415 {
00416 ROS_INFO("[all_camera_viewer] stereoModeSrvCallback");
00417 boost::mutex::scoped_lock lock(m_ServiceMutex);
00418
00419 try
00420 {
00421 right_color_image_8U3_ = cv_bridge_0_.imgMsgToCv(right_camera_data, "passthrough");
00422 left_color_image_8U3_ = cv_bridge_1_.imgMsgToCv(left_camera_data, "passthrough");
00423
00424 cv::Mat tmp = right_color_image_8U3_;
00425 right_color_mat_8U3_ = tmp.clone();
00426
00427 tmp = left_color_image_8U3_;
00428 left_color_mat_8U3_ = tmp.clone();
00429 }
00430 catch (sensor_msgs::CvBridgeException& e)
00431 {
00432 ROS_ERROR("[all_camera_viewer] Could not convert stereo images with cv_bridge.");
00433 }
00434
00435 cv::Mat right_color_8U3;
00436 cv::resize(right_color_mat_8U3_, right_color_8U3, cv::Size(), 0.5, 0.5);
00437 cv::imshow("Right color data", right_color_8U3);
00438
00439 cv::Mat left_color_8U3;
00440 cv::resize(left_color_mat_8U3_, left_color_8U3, cv::Size(), 0.5, 0.5);
00441 cv::imshow("Left color data", left_color_8U3);
00442 cv::waitKey(1000);
00443
00444 ROS_INFO("[all_camera_viewer] stereoModeSrvCallback [OK]");
00445 }
00446
00447 bool saveCameraImagesServiceCallback(cob_camera_sensors::AcquireCalibrationImages::Request &req,
00448 cob_camera_sensors::AcquireCalibrationImages::Response &res)
00449 {
00450 ROS_INFO("[all_camera_viewer] Service Callback");
00451 boost::mutex::scoped_lock lock(m_ServiceMutex);
00452
00453 if (req.reset_image_counter) image_counter_ = 0;
00454
00455 char counterBuffer [50];
00456 sprintf(counterBuffer, "%04d", image_counter_);
00457
00458 if (use_right_color_camera_ && right_color_mat_8U3_.empty())
00459 {
00460 ROS_INFO("[all_camera_viewer] Right color image not available");
00461 return false;
00462 }
00463 else
00464 {
00465 std::stringstream ss;
00466 ss << "right_color_image_";
00467 ss << counterBuffer;
00468 ss << ".bmp";
00469 cv::imwrite(absolute_output_directory_path_ + ss.str(), right_color_mat_8U3_);
00470 ROS_INFO("[all_camera_viewer] Saved right color image %d to %s", image_counter_, ss.str().c_str());
00471 }
00472
00473 if (use_left_color_camera_ && left_color_mat_8U3_.empty())
00474 {
00475 ROS_INFO("[all_camera_viewer] Left color image not available");
00476 return false;
00477 }
00478 else
00479 {
00480 std::stringstream ss;
00481 ss << "left_color_image_";
00482 ss << counterBuffer;
00483 ss << ".bmp";
00484 cv::imwrite(absolute_output_directory_path_ + ss.str(), left_color_mat_8U3_);
00485 ROS_INFO("[all_camera_viewer] Saved left color image %d to %s", image_counter_, ss.str().c_str());
00486 }
00487
00488
00489 if (use_tof_camera_ && grey_mat_8U1_.empty())
00490 {
00491 ROS_INFO("[all_camera_viewer] Tof grayscale image not available");
00492 return false;
00493 }
00494 else
00495 {
00496 std::stringstream ss;
00497 ss << "tof_grey_image_";
00498 ss << counterBuffer;
00499 ss << ".bmp";
00500 cv::imwrite(absolute_output_directory_path_ + ss.str(), grey_mat_8U1_);
00501 ROS_INFO("[all_camera_viewer] Saved tof gray image %d to %s", image_counter_, ss.str().c_str());
00502 }
00503
00504 image_counter_++;
00505
00506 return true;
00507 }
00508
00509 unsigned long loadParameters()
00510 {
00511 std::string tmp_string;
00512
00514 if (node_handle_.getParam("all_camera_viewer/use_tof_camera", use_tof_camera_) == false)
00515 {
00516 ROS_ERROR("[all_camera_viewer] 'use_tof_camera' not specified");
00517 return false;
00518 }
00519 ROS_INFO("use tof camera: %d", use_tof_camera_);
00520
00521 if (node_handle_.getParam("all_camera_viewer/use_right_color_camera", use_right_color_camera_) == false)
00522 {
00523 ROS_ERROR("[all_camera_viewer] 'use_right_color_camera' not specified");
00524 return false;
00525 }
00526 ROS_INFO("use right color camera: %d", use_right_color_camera_);
00527
00528 if (node_handle_.getParam("all_camera_viewer/use_left_color_camera", use_left_color_camera_) == false)
00529 {
00530 ROS_ERROR("[all_camera_viewer] 'use_left_color_camera' not specified");
00531 return false;
00532 }
00533 ROS_INFO("use left color camera: %d", use_left_color_camera_);
00534
00535 if (node_handle_.getParam("all_camera_viewer/absolute_output_directory_path", absolute_output_directory_path_) == false)
00536 {
00537 ROS_ERROR("[all_camera_viewer] 'absolute_output_directory_path' not specified");
00538 return false;
00539 }
00540 ROS_INFO("use left color camera: %d", use_left_color_camera_);
00541
00542 return true;
00543 }
00544
00545 };
00546
00547
00548
00549 int main(int argc, char** argv)
00550 {
00552 ros::init(argc, argv, "all_camera_viewer");
00553
00555 ros::NodeHandle nh;
00556
00558 AllCameraViewer camera_node(nh);
00559
00561 if (!camera_node.init()) return 0;
00562
00563 ros::spin();
00564
00565 return 0;
00566 }