kinect_image_flip.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_perception_common
00012  * ROS package name: cob_image_flip
00013  *                                                              
00014  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00015  *                      
00016  * Author: Richard Bormann, email:richard.bormann@ipa.fhg.de
00017  * Supervised by: Richard Bormann, email:richard.bormann@ipa.fhg.de
00018  *
00019  * Date of creation: May 2011
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 #include <cob_image_flip/kinect_image_flip.h>
00057 
00058 namespace cob_image_flip
00059 {
00060 CobKinectImageFlip::CobKinectImageFlip(ros::NodeHandle nh)
00061 {
00062         node_handle_ = nh;
00063 
00064         // set parameters
00065         std::string robot;
00066         flip_color_image_ = false;
00067         flip_pointcloud_ = false;
00068         pointcloud_data_format_ = "xyz";
00069 
00070         img_sub_counter_ = 0;
00071         pc_sub_counter_ = 0;
00072 
00073         std::cout << "\n--------------------------\nKinect Image Flip Parameters:\n--------------------------" << std::endl;
00074         node_handle_.param("flip_color_image", flip_color_image_, false);
00075         std::cout << "flip_color_image = " << flip_color_image_ << std::endl;
00076         node_handle_.param("flip_pointcloud", flip_pointcloud_, false);
00077         std::cout << "flip_pointcloud = " << flip_pointcloud_ << std::endl;
00078         node_handle_.param<std::string>("pointcloud_data_format", pointcloud_data_format_, "xyz");
00079         std::cout << "pointcloud_data_format = " << pointcloud_data_format_ << std::endl;
00080         node_handle_.param("display_warnings", display_warnings_, false);
00081         std::cout << "display_warnings = " << display_warnings_ << std::endl;
00082         node_handle_.param("display_timing", display_timing_, false);
00083         std::cout << "display_timing = " << display_timing_ << std::endl;
00084         node_handle_.param<std::string>("robot", robot, "cob3-3");
00085         std::cout << "robot = " << robot << std::endl;
00086 
00087         // determine robot number
00088         cob3Number_ = 0;
00089         //ros::get_environment_variable(value, "ROBOT");
00090         std::stringstream ss;
00091         ss << robot.substr(robot.find("cob3-")+5);
00092         ss >> cob3Number_;
00093         std::cout << "CobKinectImageFlip: Robot number is cob3-" << cob3Number_ << "." << std::endl;
00094 
00095         //sync_pointcloud_ = 0;
00096 
00097         transform_listener_ = 0;
00098 
00099 //      std::cout << "a" << std::endl;
00100 
00101         if (flip_color_image_ == true)
00102         {
00103                 it_ = new image_transport::ImageTransport(node_handle_);
00104 
00105 //              std::cout << "a1" << std::endl;
00106 
00107 
00108 //              std::cout << "a2" << std::endl;
00109 
00110                 color_camera_image_sub_.registerCallback(boost::bind(&CobKinectImageFlip::imageCallback, this, _1));
00111 
00112 //              std::cout << "a3" << std::endl;
00113 
00114                 color_camera_image_pub_ = it_->advertise("colorimage_out", 1, boost::bind(&CobKinectImageFlip::imgConnectCB, this, _1), boost::bind(&CobKinectImageFlip::imgDisconnectCB, this, _1));
00115         }
00116         else
00117         {
00118                 it_ = 0;
00119         }
00120 
00121 //      std::cout << "b" << std::endl;
00122 
00123         // point cloud flip
00124         if (flip_pointcloud_ == true)
00125         {
00126                 point_cloud_pub_ = node_handle_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 1,  boost::bind(&CobKinectImageFlip::pcConnectCB, this, _1), boost::bind(&CobKinectImageFlip::pcDisconnectCB, this, _1));
00127         }
00128 
00129 //      std::cout << "c" << std::endl;
00130 
00131         transform_listener_ = new tf::TransformListener(node_handle_);
00132 
00133         std::cout << "CobKinectImageFlip initilized." << std::endl;
00134 }
00135 
00136 CobKinectImageFlip::~CobKinectImageFlip()
00137 {
00138         if (it_ != 0)
00139                 delete it_;
00140 //              if (sync_pointcloud_ != 0)
00141 //                      delete sync_pointcloud_;
00142         if (transform_listener_ != 0)
00143                 delete transform_listener_;
00144 }
00145 
00146 unsigned long CobKinectImageFlip::init()
00147 {
00148         //color_camera_image_sub_.subscribe(*it_, "colorimage", 1);
00149         //point_cloud_sub_.subscribe(node_handle_, "pointcloud", 1);
00150 
00151         //sync_pointcloud_->connectInput(point_cloud_sub_, color_camera_image_sub_);
00152         //sync_pointcloud_callback_connection_ = sync_pointcloud_->registerCallback(boost::bind(&CobKinectImageFlipNodelet::inputCallback, this, _1, _2));
00153 
00154         return ipa_Utils::RET_OK;
00155 }
00156 
00157 unsigned long CobKinectImageFlip::convertColorImageMessageToMat(const sensor_msgs::Image::ConstPtr& color_image_msg, cv_bridge::CvImageConstPtr& color_image_ptr, cv::Mat& color_image)
00158 {
00159         try
00160         {
00161                 color_image_ptr = cv_bridge::toCvShare(color_image_msg, sensor_msgs::image_encodings::BGR8);
00162         } catch (cv_bridge::Exception& e)
00163         {
00164                 ROS_ERROR("PeopleDetection: cv_bridge exception: %s", e.what());
00165                 return ipa_Utils::RET_FAILED;
00166         }
00167         color_image = color_image_ptr->image;
00168 
00169         return ipa_Utils::RET_OK;
00170 }
00171 
00172 template <typename T>
00173 void CobKinectImageFlip::inputCallback(const sensor_msgs::PointCloud2::ConstPtr& point_cloud_msg)
00174 {
00175 //              Timer tim;
00176 //              tim.start();
00177 
00178         // check camera link orientation and decide whether image must be turned around
00179         bool turnAround = false;
00180         tf::StampedTransform transform;
00181         try
00182         {
00183                 transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
00184                 btScalar roll, pitch, yaw;
00185                 transform.getBasis().getRPY(roll, pitch, yaw, 1);
00186                 if (cob3Number_ == 2)
00187                         roll *= -1.;
00188                 if (roll > 0.0)
00189                         turnAround = true;
00190                 //      std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
00191                 //      std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
00192                 //      std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
00193         } catch (tf::TransformException ex)
00194         {
00195                 if (display_warnings_ == true)
00196                         ROS_WARN("%s",ex.what());
00197         }
00198 
00199         if (turnAround == false)
00200         {
00201                 // image upright
00202                 //sensor_msgs::Image color_image_turned_msg = *color_image_msg;
00203                 //color_image_turned_msg.header.stamp = ros::Time::now();
00204                 //color_camera_image_pub_.publish(color_image_turned_msg);
00205                 //sensor_msgs::PointCloud2::ConstPtr point_cloud_turned_msg = point_cloud_msg;
00206                 point_cloud_pub_.publish(point_cloud_msg);
00207         }
00208         else
00209         {
00210                 // image upside down
00211                 // point cloud
00212                 pcl::PointCloud<T> point_cloud_src;
00213                 pcl::fromROSMsg(*point_cloud_msg, point_cloud_src);
00214 
00215                 boost::shared_ptr<pcl::PointCloud<T> > point_cloud_turned(new pcl::PointCloud<T>);
00216                 point_cloud_turned->header = point_cloud_msg->header;
00217                 point_cloud_turned->height = point_cloud_msg->height;
00218                 point_cloud_turned->width = point_cloud_msg->width;
00219                 //point_cloud_turned->sensor_orientation_ = point_cloud_msg->sensor_orientation_;
00220                 //point_cloud_turned->sensor_origin_ = point_cloud_msg->sensor_origin_;
00221                 point_cloud_turned->is_dense = true;    //point_cloud_msg->is_dense;
00222                 point_cloud_turned->resize(point_cloud_src.height*point_cloud_src.width);
00223                 for (int v = (int)point_cloud_src.height - 1; v >= 0; v--)
00224                 {
00225                         for (int u = (int)point_cloud_src.width - 1; u >= 0; u--)
00226                         {
00227                                 (*point_cloud_turned)(point_cloud_src.width-1 - u, point_cloud_src.height-1 - v) = point_cloud_src(u, v);
00228                         }
00229                 }
00230 
00231                 // publish turned data
00232                 sensor_msgs::PointCloud2::Ptr point_cloud_turned_msg(new sensor_msgs::PointCloud2);
00233                 pcl::toROSMsg(*point_cloud_turned, *point_cloud_turned_msg);
00234                 //point_cloud_turned_msg->header.stamp = ros::Time::now();
00235                 point_cloud_pub_.publish(point_cloud_turned_msg);
00236 
00237                 //      cv::namedWindow("test");
00238                 //      cv::imshow("test", color_image_turned);
00239                 //      cv::waitKey(10);
00240         }
00241 
00242         if (display_timing_ == true)
00243                 ROS_INFO("%d ImageFlip: Time stamp of pointcloud message: %f. Delay: %f.", point_cloud_msg->header.seq, point_cloud_msg->header.stamp.toSec(), ros::Time::now().toSec()-point_cloud_msg->header.stamp.toSec());
00244 //              ROS_INFO("Pointcloud callback in image flip took %f ms.", tim.getElapsedTimeInMilliSec());
00245 }
00246 
00247 void CobKinectImageFlip::imageCallback(const sensor_msgs::ImageConstPtr& color_image_msg)
00248 {
00249 //              Timer tim;
00250 //              tim.start();
00251 
00252         // check camera link orientation and decide whether image must be turned around
00253         bool turnAround = false;
00254         tf::StampedTransform transform;
00255         try
00256         {
00257                 transform_listener_->lookupTransform("/base_link", "/head_cam3d_link", ros::Time(0), transform);
00258                 btScalar roll, pitch, yaw;
00259                 transform.getBasis().getRPY(roll, pitch, yaw, 1);
00260                 if (cob3Number_ == 2)
00261                         roll *= -1.;
00262                 if (roll > 0.0)
00263                         turnAround = true;
00264                 //      std::cout << "xyz: " << transform.getOrigin().getX() << " " << transform.getOrigin().getY() << " " << transform.getOrigin().getZ() << "\n";
00265                 //      std::cout << "abcw: " << transform.getRotation().getX() << " " << transform.getRotation().getY() << " " << transform.getRotation().getZ() << " " << transform.getRotation().getW() << "\n";
00266                 //      std::cout << "rpy: " << roll << " " << pitch << " " << yaw << "\n";
00267         } catch (tf::TransformException ex)
00268         {
00269                 if (display_warnings_ == true)
00270                         ROS_WARN("%s",ex.what());
00271         }
00272 
00273         // now turn if necessary
00274         if (turnAround == false)
00275         {
00276                 // image upright
00277                 sensor_msgs::Image color_image_turned_msg = *color_image_msg;
00278                 color_camera_image_pub_.publish(color_image_turned_msg);
00279         }
00280         else
00281         {
00282                 // image upside down
00283                 cv_bridge::CvImageConstPtr color_image_ptr;
00284                 cv::Mat color_image;
00285                 convertColorImageMessageToMat(color_image_msg, color_image_ptr, color_image);
00286 
00287                 // rotate images by 180 degrees
00288                 cv::Mat color_image_turned(color_image.rows, color_image.cols, color_image.type());
00289                 if (color_image.type() != CV_8UC3)
00290                 {
00291                         std::cout << "CobKinectImageFlipNodelet::inputCallback: Error: The image format of the color image is not CV_8UC3.\n";
00292                         return;
00293                 }
00294                 for (int v = 0; v < color_image.rows; v++)
00295                 {
00296                         uchar* src = color_image.ptr(v);
00297                         uchar* dst = color_image_turned.ptr(color_image.rows - v - 1);
00298                         dst += 3 * (color_image.cols - 1);
00299                         for (int u = 0; u < color_image.cols; u++)
00300                         {
00301                                 for (int k = 0; k < 3; k++)
00302                                 {
00303                                         *dst = *src;
00304                                         src++;
00305                                         dst++;
00306                                 }
00307                                 dst -= 6;
00308                         }
00309                 }
00310                 cv_bridge::CvImage cv_ptr;
00311                 cv_ptr.image = color_image_turned;
00312                 cv_ptr.encoding = "bgr8";
00313                 sensor_msgs::Image::Ptr color_image_turned_msg = cv_ptr.toImageMsg();
00314                 color_image_turned_msg->header = color_image_msg->header;
00315                 color_camera_image_pub_.publish(color_image_turned_msg);
00316         }
00317 
00318         //ROS_INFO("Image callback in image flip took %f ms.", tim.getElapsedTimeInMilliSec());
00319 }
00320 
00321 void CobKinectImageFlip::imgConnectCB(const image_transport::SingleSubscriberPublisher& pub)
00322   {
00323     img_sub_counter_++;
00324     if(img_sub_counter_ == 1)
00325     {
00326       ROS_DEBUG("connecting");
00327       color_camera_image_sub_.subscribe(*it_, "colorimage_in", 1);
00328     }
00329   }
00330 
00331 void CobKinectImageFlip::imgDisconnectCB(const image_transport::SingleSubscriberPublisher& pub)
00332   {
00333     img_sub_counter_--;
00334     if(img_sub_counter_ == 0)
00335     {
00336       ROS_DEBUG("disconnecting");
00337       color_camera_image_sub_.unsubscribe();
00338     }
00339   }
00340 
00341 void CobKinectImageFlip::pcConnectCB(const ros::SingleSubscriberPublisher& pub)
00342   {
00343     pc_sub_counter_++;
00344     if(pc_sub_counter_ == 1)
00345     {
00346       ROS_DEBUG("connecting");
00347       if (pointcloud_data_format_.compare("xyz") == 0)
00348         point_cloud_sub_ = node_handle_.subscribe<sensor_msgs::PointCloud2>("pointcloud_in", 1, &CobKinectImageFlip::inputCallback<pcl::PointXYZ>, this);
00349       else if (pointcloud_data_format_.compare("xyzrgb") == 0)
00350         point_cloud_sub_ = node_handle_.subscribe<sensor_msgs::PointCloud2>("pointcloud_in", 1, &CobKinectImageFlip::inputCallback<pcl::PointXYZRGB>, this);
00351       else
00352       {
00353         ROS_ERROR("Unknown pointcloud format specified in the paramter file.");
00354         pc_sub_counter_ = 0;
00355       }
00356     }
00357   }
00358 
00359 void CobKinectImageFlip::pcDisconnectCB(const ros::SingleSubscriberPublisher& pub)
00360   {
00361     pc_sub_counter_--;
00362     if(pc_sub_counter_ == 0)
00363     {
00364       ROS_DEBUG("disconnecting");
00365       point_cloud_sub_.shutdown();
00366     }
00367   }
00368 
00369 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


cob_image_flip
Author(s): Richard Bormann
autogenerated on Fri Mar 8 2013 15:03:53