tof_camera_viewer.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_camera_sensors
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
00017  * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
00018  *
00019  * Date of creation: Jan 2010
00020  * ToDo:
00021  *
00022  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00023  *
00024  * Redistribution and use in source and binary forms, with or without
00025  * modification, are permitted provided that the following conditions are met:
00026  *
00027  *     * Redistributions of source code must retain the above copyright
00028  *       notice, this list of conditions and the following disclaimer.
00029  *     * Redistributions in binary form must reproduce the above copyright
00030  *       notice, this list of conditions and the following disclaimer in the
00031  *       documentation and/or other materials provided with the distribution.
00032  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00033  *       Engineering and Automation (IPA) nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as 
00039  * published by the Free Software Foundation, either version 3 of the 
00040  * License, or (at your option) any later version.
00041  * 
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL for more details.
00046  * 
00047  * You should have received a copy of the GNU Lesser General Public 
00048  * License LGPL along with this program. 
00049  * If not, see <http://www.gnu.org/licenses/>.
00050  *
00051  ****************************************************************/
00052 
00053 //##################
00054 //#### includes ####
00055 
00056 // ROS includes
00057 #include <ros/ros.h>
00058 #include <cv_bridge/CvBridge.h>
00059 #include <image_transport/image_transport.h>
00060 
00061 // ROS message includes
00062 #include <sensor_msgs/Image.h>
00063 
00064 // External includes
00065 #include <opencv/cv.h>
00066 #include <opencv/highgui.h>
00067 
00068 #include <cob_vision_utils/VisionUtils.h>
00069 #include <sstream>
00070 
00071 //####################
00072 //#### node class ####
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 //#### main programm ####
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  int numPoints = image->width*image->height;
00220  sensor_msgs::PointCloud pc_msg;
00221  pc_msg.header.stamp = ros::Time::now(); 
00222  pc_msg.points.resize(numPoints);
00223                
00225  for (int i = 0; i < numPoints; i++)
00226  {
00227  msg.points[i].x = ((float*)image->imageData)[3*i + 0];
00228  msg.points[i].y = ((float*)image->imageData)[3*i + 1];
00229  msg.points[i].z = ((float*)image->imageData)[3*i + 2];
00230  }
00231 */
00232 /*
00233 IplImage* image = cvCreateImage(cvSize(176, 144), IPL_DEPTH_32F, 3);
00234  for (unsigned int i=0; i<msg->points.size(); i++)
00235  {
00236  ((float*)image->imageData)[3*i+0] = (msg->points[i]).x;
00237  ((float*)image->imageData)[3*i+1] = (msg->points[i]).y;
00238  ((float*)image->imageData)[3*i+2] = (msg->points[i]).z;
00239  }
00240  IplImage* image_show = cvCreateImage(cvSize(176, 144), IPL_DEPTH_8U, 3);
00241  ipa_Utils::ConvertToShowImage(image, image_show, 3);
00242 */


cob_camera_sensors
Author(s): Jan Fischer
autogenerated on Thu Aug 27 2015 12:45:58