$search
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_point_cloud_publisher 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00018 * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00019 * 00020 * Date of creation: August 2010 00021 * ToDo: 00022 * 00023 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00024 * 00025 * Redistribution and use in source and binary forms, with or without 00026 * modification, are permitted provided that the following conditions are met: 00027 * 00028 * * Redistributions of source code must retain the above copyright 00029 * notice, this list of conditions and the following disclaimer. 00030 * * Redistributions in binary form must reproduce the above copyright 00031 * notice, this list of conditions and the following disclaimer in the 00032 * documentation and/or other materials provided with the distribution. 00033 * * Neither the name of the Fraunhofer Institute for Manufacturing 00034 * Engineering and Automation (IPA) nor the names of its 00035 * contributors may be used to endorse or promote products derived from 00036 * this software without specific prior written permission. 00037 * 00038 * This program is free software: you can redistribute it and/or modify 00039 * it under the terms of the GNU Lesser General Public License LGPL as 00040 * published by the Free Software Foundation, either version 3 of the 00041 * License, or (at your option) any later version. 00042 * 00043 * This program is distributed in the hope that it will be useful, 00044 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00045 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00046 * GNU Lesser General Public License LGPL for more details. 00047 * 00048 * You should have received a copy of the GNU Lesser General Public 00049 * License LGPL along with this program. 00050 * If not, see <http://www.gnu.org/licenses/>. 00051 * 00052 ****************************************************************/ 00053 00054 //################## 00055 //#### includes #### 00056 00057 // ROS includes 00058 #include <ros/ros.h> 00059 #include <cv_bridge/cv_bridge.h> 00060 #include <sensor_msgs/image_encodings.h> 00061 #include <image_transport/image_transport.h> 00062 #include <image_transport/subscriber_filter.h> 00063 #include <message_filters/sync_policies/approximate_time.h> 00064 #include <message_filters/synchronizer.h> 00065 #include <message_filters/subscriber.h> 00066 00067 #include <pluginlib/class_list_macros.h> 00068 #include <nodelet/nodelet.h> 00069 00070 // ROS message includes 00071 #include <sensor_msgs/Image.h> 00072 #include <sensor_msgs/PointCloud2.h> 00073 #include <std_msgs/Float64.h> 00074 00075 using namespace message_filters; 00076 00077 typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> SyncPolicy; 00078 00079 00080 //#################### 00081 //#### PcPublisher class #### 00082 class CreateColoredPointCloud : public nodelet::Nodelet 00083 { 00084 private: 00085 ros::NodeHandle n_; 00086 00087 // topics to subscribe, callback is called for new messages arriving 00088 image_transport::ImageTransport image_transport_; 00089 image_transport::SubscriberFilter color_image_sub_; 00090 message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub_; 00091 00092 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > pc_sync_; 00093 00094 //sensor_msgs::CvBridge cv_bridge_0_; ///< Converts ROS image messages to openCV IplImages 00095 cv::Mat c_color_image_8U3_; 00096 00097 // topics to publish 00098 ros::Publisher pc_pub_; 00099 00100 int num_subs; 00101 00102 public: 00103 // Constructor 00104 CreateColoredPointCloud() 00105 : image_transport_(n_), 00106 num_subs(0) 00107 {} 00108 00109 // Destructor 00110 ~CreateColoredPointCloud() 00111 { 00112 } 00113 00114 void initNode() 00115 { 00116 pc_sync_ = boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(3))); 00117 pc_pub_ = n_.advertise<sensor_msgs::PointCloud2>("colored_point_cloud2", 1, boost::bind(&CreateColoredPointCloud::connectCb, this), boost::bind(&CreateColoredPointCloud::disconnectCb, this)); 00118 //color_image_sub_.subscribe(image_transport_,"image_color", 1); 00119 //pc_sub_.subscribe(n_, "point_cloud2", 1); 00120 00121 pc_sync_->connectInput(color_image_sub_, pc_sub_); 00122 pc_sync_->registerCallback(boost::bind(&CreateColoredPointCloud::syncCallback, this, _1, _2)); 00123 } 00124 00125 virtual void onInit() 00126 { 00127 n_ = getNodeHandle(); 00128 image_transport_ = image_transport::ImageTransport(n_); 00129 initNode(); 00130 } 00131 00132 00133 void connectCb() 00134 { 00135 num_subs++; 00136 if(num_subs > 0) 00137 { 00138 color_image_sub_.subscribe(image_transport_,"image_color", 1); 00139 pc_sub_.subscribe(n_, "point_cloud2", 1); 00140 } 00141 } 00142 00143 void disconnectCb() 00144 { 00145 num_subs--; 00146 if(num_subs <= 0) 00147 { 00148 color_image_sub_.unsubscribe(); 00149 pc_sub_.unsubscribe(); 00150 } 00151 } 00152 00153 00154 // topic callback functions 00155 // function will be called when a new message arrives on a topic 00156 void syncCallback(const sensor_msgs::Image::ConstPtr& img_rgb, const sensor_msgs::PointCloud2::ConstPtr& pc) 00157 { 00158 sensor_msgs::PointCloud2 cpc_msg; 00159 // create point_cloud message 00160 cpc_msg.header = pc->header; 00161 00162 cv_bridge::CvImagePtr cv_ptr; 00163 try 00164 { 00165 cv_ptr = cv_bridge::toCvCopy(img_rgb,sensor_msgs::image_encodings::BGR8); 00166 } 00167 catch (cv_bridge::Exception& e) 00168 { 00169 ROS_ERROR("cv_bridge exception: %s", e.what()); 00170 return; 00171 } 00172 //c_color_image_8U3_ = cv_ptr->image;//cv_bridge_0_.imgMsgToCv(img_rgb, "passthrough"); 00173 //cv::Mat cpp_color_image_8U3 = c_color_image_8U3_; 00174 00175 cpc_msg.width = pc->width; 00176 cpc_msg.height = pc->height; 00177 cpc_msg.fields.resize(4); 00178 cpc_msg.fields[0].name = "x"; 00179 cpc_msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32; 00180 cpc_msg.fields[1].name = "y"; 00181 cpc_msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32; 00182 cpc_msg.fields[2].name = "z"; 00183 cpc_msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32; 00184 cpc_msg.fields[3].name = "rgb"; 00185 cpc_msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32; 00186 int offset = 0; 00187 for (size_t d = 0; d < cpc_msg.fields.size(); d++, offset += 4) 00188 { 00189 cpc_msg.fields[d].offset = offset; 00190 } 00191 cpc_msg.point_step = offset; 00192 cpc_msg.row_step = cpc_msg.point_step * cpc_msg.width; 00193 cpc_msg.data.resize (cpc_msg.width*cpc_msg.height*cpc_msg.point_step); 00194 cpc_msg.is_dense = true; 00195 cpc_msg.is_bigendian = false; 00196 00197 unsigned char* c_ptr = 0; 00198 int cpc_msg_idx=0; 00199 for (int row = 0; row < cv_ptr->image.rows; row++) 00200 { 00201 c_ptr = cv_ptr->image.ptr<unsigned char>(row); 00202 //TODO: remove filtered points 00203 for (int col = 0; col < cv_ptr->image.cols; col++, cpc_msg_idx++) 00204 { 00205 memcpy(&cpc_msg.data[cpc_msg_idx * cpc_msg.point_step], &pc->data[cpc_msg_idx * pc->point_step], 3*sizeof(float)); 00206 memcpy(&cpc_msg.data[cpc_msg_idx * cpc_msg.point_step + cpc_msg.fields[3].offset], &c_ptr[3*col], 3*sizeof(unsigned char)); 00207 } 00208 } 00209 pc_pub_.publish(cpc_msg); 00210 } 00211 00212 }; 00213 00214 //####################### 00215 //#### main programm #### 00216 int main(int argc, char** argv) 00217 { 00218 // initialize ROS, specify name of node 00219 ros::init(argc, argv, "cpc_publisher"); 00220 00221 CreateColoredPointCloud cpcPublisher; 00222 cpcPublisher.initNode(); 00223 00224 ros::spin(); 00225 00226 return 0; 00227 } 00228 00229 00230 PLUGINLIB_DECLARE_CLASS(cob_gazebo, CreateColoredPointCloud, CreateColoredPointCloud, nodelet::Nodelet); 00231 00232 00233