Go to the documentation of this file.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 #include <ros/ros.h>
00058 #include <cv_bridge/CvBridge.h>
00059 #include <image_transport/image_transport.h>
00060
00061
00062 #include <sensor_msgs/Image.h>
00063
00064
00065 #include <opencv/cv.h>
00066 #include <opencv/highgui.h>
00067
00068 #include <cob_vision_utils/VisionUtils.h>
00069 #include <sstream>
00070
00071
00072
00073 class CobTofCameraViewerNode
00074 {
00075 private:
00076 ros::NodeHandle m_NodeHandle;
00077
00078 image_transport::ImageTransport image_transport_;
00079 image_transport::Subscriber xyz_image_subscriber_;
00080 image_transport::Subscriber grey_image_subscriber_;
00081
00082 sensor_msgs::CvBridge cv_bridge_0_;
00083 sensor_msgs::CvBridge cv_bridge_1_;
00084
00085 IplImage* xyz_image_32F3_;
00086 cv::Mat xyz_mat_8U3_;
00087 IplImage* grey_image_32F1_;
00088 cv::Mat grey_mat_8U3_;
00089
00090 int grey_image_counter_;
00091
00092 public:
00095 CobTofCameraViewerNode(const ros::NodeHandle& node_handle)
00096 : m_NodeHandle(node_handle),
00097 image_transport_(node_handle),
00098 xyz_image_32F3_(0),
00099 xyz_mat_8U3_(cv::Mat()),
00100 grey_image_32F1_(0),
00101 grey_mat_8U3_(cv::Mat()),
00102 grey_image_counter_(0)
00103 {
00105 }
00106
00108 ~CobTofCameraViewerNode()
00109 {
00113
00114 if(cvGetWindowHandle("z data"))cvDestroyWindow("z data");
00115 if(cvGetWindowHandle("grey data"))cvDestroyWindow("grey data");
00116 }
00117
00120 bool init()
00121 {
00123 cvStartWindowThread();
00124 cv::namedWindow("z data");
00125 cv::namedWindow("grey data");
00126
00127 xyz_image_subscriber_ = image_transport_.subscribe("image_xyz", 1, &CobTofCameraViewerNode::xyzImageCallback, this);
00128 grey_image_subscriber_ = image_transport_.subscribe("image_grey", 1, &CobTofCameraViewerNode::greyImageCallback, this);
00129
00130 return true;
00131 }
00132
00136 void greyImageCallback(const sensor_msgs::ImageConstPtr& grey_image_msg)
00137 {
00140 ROS_INFO("Grey Image Callback");
00141
00142 try
00143 {
00144 grey_image_32F1_ = cv_bridge_0_.imgMsgToCv(grey_image_msg, "passthrough");
00145
00146 cv::Mat grey_mat_32F1 = grey_image_32F1_;
00147 ipa_Utils::ConvertToShowImage(grey_mat_32F1, grey_mat_8U3_, 1, 0, 800);
00148 cv::imshow("grey data", grey_mat_8U3_);
00149 int c = cvWaitKey(500);
00150 std::cout << c << std::endl;
00151 if (c=='s' || c==536871027)
00152 {
00153 std::stringstream ss;
00154 char counterBuffer [50];
00155 sprintf(counterBuffer, "%04d", grey_image_counter_);
00156 ss << "greyImage8U3_";
00157 ss << counterBuffer;
00158 ss << ".bmp";
00159 cv::imwrite(ss.str().c_str(),grey_mat_8U3_);
00160 std::cout << "[tof_camera_viewer] Image " << grey_image_counter_ << " saved." << std::endl;
00161 grey_image_counter_++;
00162 }
00163 }
00164 catch (sensor_msgs::CvBridgeException& e)
00165 {
00166 ROS_ERROR("[tof_camera_viewer] Could not convert from '%s' to '32FC1'.", grey_image_msg->encoding.c_str());
00167 }
00168 ROS_INFO("Image Processed");
00169 }
00170
00174 void xyzImageCallback(const sensor_msgs::ImageConstPtr& xyz_image_msg)
00175 {
00178
00179 try
00180 {
00181 xyz_image_32F3_ = cv_bridge_1_.imgMsgToCv(xyz_image_msg, "passthrough");
00182
00183 cv::Mat xyz_mat_32F3 = xyz_image_32F3_;
00184 ipa_Utils::ConvertToShowImage(xyz_mat_32F3, xyz_mat_8U3_, 3);
00185 cv::imshow("z data", xyz_mat_8U3_);
00186 }
00187 catch (sensor_msgs::CvBridgeException& e)
00188 {
00189 ROS_ERROR("[tof_camera_viewer] Could not convert from '%s' to '32FC1'.", xyz_image_msg->encoding.c_str());
00190 }
00191
00192 }
00193 };
00194
00195
00196
00197 int main(int argc, char** argv)
00198 {
00200 ros::init(argc, argv, "tof_camera_viewer");
00201
00203 ros::NodeHandle nh;
00204
00206 CobTofCameraViewerNode camera_viewer_node(nh);
00207
00208
00210 if (!camera_viewer_node.init()) return 0;
00211
00212 ros::spin();
00213
00214 return 0;
00215 }
00216
00217
00218
00219
00220
00221
00222
00223
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242