tof_camera_viewer.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering   
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob_driver
 * ROS package name: cob_camera_sensors
 *                                                              
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *                      
 * Author: Jan Fischer, email:jan.fischer@ipa.fhg.de
 * Supervised by: Jan Fischer, email:jan.fischer@ipa.fhg.de
 *
 * Date of creation: Jan 2010
 * ToDo:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Fraunhofer Institute for Manufacturing 
 *       Engineering and Automation (IPA) nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 of the 
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

//##################
//#### includes ####

// ROS includes
#include <ros/ros.h>
#include <cv_bridge/CvBridge.h>
#include <image_transport/image_transport.h>

// ROS message includes
#include <sensor_msgs/Image.h>

// External includes
#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cob_vision_utils/VisionUtils.h>
#include <sstream>

//####################
//#### node class ####
class CobTofCameraViewerNode
{
private:
        ros::NodeHandle m_NodeHandle;   

        image_transport::ImageTransport image_transport_;       
        image_transport::Subscriber xyz_image_subscriber_;      
        image_transport::Subscriber grey_image_subscriber_;     

        sensor_msgs::CvBridge cv_bridge_0_;
        sensor_msgs::CvBridge cv_bridge_1_;
        
        IplImage* xyz_image_32F3_;      
        cv::Mat xyz_mat_8U3_;           
        IplImage* grey_image_32F1_;     
        cv::Mat grey_mat_8U3_;  

        int grey_image_counter_; 

public:
        CobTofCameraViewerNode(const ros::NodeHandle& node_handle)
        : m_NodeHandle(node_handle),
          image_transport_(node_handle),
          xyz_image_32F3_(0),
          xyz_mat_8U3_(cv::Mat()),
          grey_image_32F1_(0),
          grey_mat_8U3_(cv::Mat()),
          grey_image_counter_(0)
        {
        }

        ~CobTofCameraViewerNode()
        {

                if(cvGetWindowHandle("z data"))cvDestroyWindow("z data");
                if(cvGetWindowHandle("grey data"))cvDestroyWindow("grey data");
        }

        bool init()
        {
                cvStartWindowThread();
                cv::namedWindow("z data");              
                cv::namedWindow("grey data");           

                xyz_image_subscriber_ = image_transport_.subscribe("image_xyz", 1, &CobTofCameraViewerNode::xyzImageCallback, this);
                grey_image_subscriber_ = image_transport_.subscribe("image_grey", 1, &CobTofCameraViewerNode::greyImageCallback, this);

                return true;
        }

        void greyImageCallback(const sensor_msgs::ImageConstPtr& grey_image_msg)
        {
                ROS_INFO("Grey Image Callback");

                try
                {
                        grey_image_32F1_ = cv_bridge_0_.imgMsgToCv(grey_image_msg, "passthrough");

                        cv::Mat grey_mat_32F1 = grey_image_32F1_;
                        ipa_Utils::ConvertToShowImage(grey_mat_32F1, grey_mat_8U3_, 1, 0, 800);
                        cv::imshow("grey data", grey_mat_8U3_);
                        int c = cvWaitKey(500);
                        std::cout << c << std::endl;
                        if (c=='s' || c==536871027)
                        {
                                std::stringstream ss;
                                char counterBuffer [50];
                                sprintf(counterBuffer, "%04d", grey_image_counter_);
                                ss << "greyImage8U3_";
                                ss << counterBuffer;
                                ss << ".bmp";
                                cv::imwrite(ss.str().c_str(),grey_mat_8U3_);
                                std::cout << "[tof_camera_viewer] Image " << grey_image_counter_ << " saved." << std::endl;
                                grey_image_counter_++;
                        }
                }
                catch (sensor_msgs::CvBridgeException& e)
                {
                        ROS_ERROR("[tof_camera_viewer] Could not convert from '%s' to '32FC1'.", grey_image_msg->encoding.c_str());
                }
                ROS_INFO("Image Processed");
        }

        void xyzImageCallback(const sensor_msgs::ImageConstPtr& xyz_image_msg)
        {

                try
                {
                        xyz_image_32F3_ = cv_bridge_1_.imgMsgToCv(xyz_image_msg, "passthrough");
                        
                        cv::Mat xyz_mat_32F3 = xyz_image_32F3_;
                        ipa_Utils::ConvertToShowImage(xyz_mat_32F3, xyz_mat_8U3_, 3);
                        cv::imshow("z data", xyz_mat_8U3_);
                }
                catch (sensor_msgs::CvBridgeException& e)
                {
                        ROS_ERROR("[tof_camera_viewer] Could not convert from '%s' to '32FC1'.", xyz_image_msg->encoding.c_str());
                }

        }
};

//#######################
//#### main programm ####
int main(int argc, char** argv)
{
        ros::init(argc, argv, "tof_camera_viewer");

        ros::NodeHandle nh;

        CobTofCameraViewerNode camera_viewer_node(nh);


        if (!camera_viewer_node.init()) return 0;

        ros::spin();

        return 0;
}


/*
 int numPoints = image->width*image->height;
 sensor_msgs::PointCloud pc_msg;
 pc_msg.header.stamp = ros::Time::now(); 
 pc_msg.points.resize(numPoints);
               
 for (int i = 0; i < numPoints; i++)
 {
 msg.points[i].x = ((float*)image->imageData)[3*i + 0];
 msg.points[i].y = ((float*)image->imageData)[3*i + 1];
 msg.points[i].z = ((float*)image->imageData)[3*i + 2];
 }
*/
/*
IplImage* image = cvCreateImage(cvSize(176, 144), IPL_DEPTH_32F, 3);
 for (unsigned int i=0; i<msg->points.size(); i++)
 {
 ((float*)image->imageData)[3*i+0] = (msg->points[i]).x;
 ((float*)image->imageData)[3*i+1] = (msg->points[i]).y;
 ((float*)image->imageData)[3*i+2] = (msg->points[i]).z;
 }
 IplImage* image_show = cvCreateImage(cvSize(176, 144), IPL_DEPTH_8U, 3);
 ipa_Utils::ConvertToShowImage(image, image_show, 3);
*/