00001 #include "zyonz_tof_color_driver_node.h"
00002 #include <string>
00003 #include <iostream>
00004
00005
00006 #include <opencv/cv.h>
00007 #include <opencv/highgui.h>
00008 #include <cv_bridge/CvBridge.h>
00009 #include <boost/format.hpp>
00010
00011 ZyonzTofColorDriverNode::ZyonzTofColorDriverNode(ros::NodeHandle &nh) : iri_base_driver::IriBaseNodeDriver<ZyonzTofColorDriver>(nh),
00012 depth_sent_ (true), rgb_sent_ (true), intens_sent_ (true), zbuffer_threshold_(20), saveFile (false), saveFileCounter (0)
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 kernel_size_=3;
00048
00049
00050
00051 K_<< 791.241, 0, 323.613, 0, 791.241, 266.511, 0, 0, 1;
00052 P_ << 0.987, -0.028, -0.156, 54.0775, 0.0322, 0.999, 0.0209, 2.0674, 0.156, -0.0257, 0.988, 18.3983, 0, 0, 0, 1;
00053
00054 this->cloud2_raw_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud2>("cloud2_raw", 10);
00055
00056
00057 this->input_intens_subscriber_ = this->public_node_handle_.subscribe("input_intensity", 5, &ZyonzTofColorDriverNode::input_intens_callback, this);
00058
00059
00060
00061 this->image_subscriber_ = this->public_node_handle_.subscribe("input_color", 10, &ZyonzTofColorDriverNode::image_callback, this);
00062 this->pointCloud_subscriber_ = this->public_node_handle_.subscribe("input_pointcloud", 10, &ZyonzTofColorDriverNode::pointCloud_callback, this);
00063
00064
00065 this->saveImage_server_ = this->public_node_handle_.advertiseService("saveImage", &ZyonzTofColorDriverNode::saveImageCallback, this);
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 }
00076
00077 void ZyonzTofColorDriverNode::mainNodeThread(void)
00078 {
00079
00080 this->driver_.lock();
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 this->driver_.unlock();
00098 }
00099
00100
00101 void ZyonzTofColorDriverNode::input_intens_callback(const sensor_msgs::Image::ConstPtr& msg)
00102 {
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115 intens_image_ = msg;
00116 intens_sent_ = false;
00117 processRgbAndDepth ();
00118 }
00119 void ZyonzTofColorDriverNode::image_callback(const sensor_msgs::Image::ConstPtr& msg)
00120 {
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134 rgb_image_ = msg;
00135 rgb_sent_ = false;
00136 processRgbAndDepth ();
00137 }
00138 void ZyonzTofColorDriverNode::pointCloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
00139 {
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153 cloud2_ = msg;
00154 depth_sent_ = false;
00155 processRgbAndDepth ();
00156
00157 }
00158
00159
00160 void ZyonzTofColorDriverNode::processRgbAndDepth() {
00161 if ((!rgb_sent_) && (!depth_sent_) && (!intens_sent_)){
00162
00163 MatrixXf Z_buffer_=MatrixXf::Zero(rgb_image_->width,rgb_image_->height);
00164
00165
00166 if (cloud2_raw_publisher_.getNumSubscribers () > 0)
00167 {
00168 ROS_DEBUG("Image Size %d %d %d",rgb_image_->width,rgb_image_->height,rgb_image_->step);
00169 ROS_DEBUG("Cloud Size %d %d",cloud2_->width,cloud2_->height);
00170
00171
00172
00173
00174 PointCloud2_msg_.height = cloud2_->height;
00175 PointCloud2_msg_.width = cloud2_->width;
00176 PointCloud2_msg_.fields.resize (4);
00177 PointCloud2_msg_.fields[0].name = "x";
00178 PointCloud2_msg_.fields[1].name = "y";
00179 PointCloud2_msg_.fields[2].name = "z";
00180 PointCloud2_msg_.fields[3].name = "rgb";
00181 PointCloud2_msg_.header.stamp = cloud2_->header.stamp;
00182 PointCloud2_msg_.header.frame_id = std::string ("/colorcam_frame");
00183
00184 int offset = 0;
00185 for (size_t s = 0; s < PointCloud2_msg_.fields.size (); ++s, offset += 4)
00186 {
00187 PointCloud2_msg_.fields[s].offset = offset;
00188 PointCloud2_msg_.fields[s].count = 1;
00189 PointCloud2_msg_.fields[s].datatype = sensor_msgs::PointField::FLOAT32;
00190 }
00191
00192 PointCloud2_msg_.point_step = offset;
00193 PointCloud2_msg_.row_step = PointCloud2_msg_.point_step * PointCloud2_msg_.width;
00194 PointCloud2_msg_.data.resize (PointCloud2_msg_.row_step * PointCloud2_msg_.height);
00195 PointCloud2_msg_.is_dense = true;
00196
00197
00198
00199 for (uint8_t i=0+kernel_size_;i<cloud2_->width-kernel_size_;i++)
00200 {
00201 for (uint8_t j=0+kernel_size_;j<cloud2_->height-kernel_size_;j++)
00202 {
00203 Vector4f point3D;
00204 int index2 = (i)*cloud2_->width +(j);
00205 float *pstep2 = (float*)&cloud2_->data[(index2) * cloud2_->point_step];
00206
00207
00208 point3D(0)=pstep2[0]*1000;
00209 point3D(1)=pstep2[1]*1000;
00210 point3D(2)=pstep2[2]*1000;
00211 point3D(3)=1.0;
00212
00213
00214
00215 Vector4f tmp=P_*point3D;
00216
00217 MatrixXf Kextended(3,4);
00218
00219 Kextended<<K_,Vector3f::Zero(3);
00220
00221 Vector3f projectedPoint=Kextended*tmp;
00222
00223
00224
00225 int u=(int)(projectedPoint(0)/projectedPoint(2));
00226 int v=(int)(projectedPoint(1)/projectedPoint(2));
00227
00228
00229
00230
00231 if ((u>kernel_size_) && (u < (int)rgb_image_->width-kernel_size_) && (v>kernel_size_) && (v < (int)rgb_image_->height-kernel_size_))
00232 {
00233 if ((Z_buffer_(u,v)==0.0) ||(tmp(2)<Z_buffer_(u,v)))
00234 {
00235
00236
00237 Z_buffer_.block(u-kernel_size_,v-kernel_size_,2*kernel_size_+1,2*kernel_size_+1)= MatrixXf::Constant(2*kernel_size_+1,2*kernel_size_+1, tmp(2));
00238 }
00239 }
00240 }
00241 }
00242
00243
00244 for (uint i=0+kernel_size_;i<cloud2_->width-kernel_size_;i++)
00245 {
00246 for (uint j=0+kernel_size_;j<cloud2_->height-kernel_size_;j++)
00247 {
00248 Vector4f point3D;
00249
00250 int index2 = (j)*cloud2_->width +(i);
00251 float *pstep = (float*)&PointCloud2_msg_.data[(index2) * PointCloud2_msg_.point_step];
00252 float *pstep2 = (float*)&cloud2_->data[(index2) * cloud2_->point_step];
00253
00254
00255 if (saveFile)
00256 outFile<<pstep2[0]<<" "<<pstep2[1]<<" "<<pstep2[2]<<" "<<0<<" "<<0<<" ";
00257
00258
00259
00260
00261
00262 point3D(0)=pstep2[0]*1000;
00263 point3D(1)=pstep2[1]*1000;
00264 point3D(2)=pstep2[2]*1000;
00265 point3D(3)=1.0;
00266
00267
00268
00269 Vector4f tmp=P_*point3D;
00270
00271
00272
00273
00274 pstep[0] = tmp(0)/1000;
00275 pstep[1] = tmp(1)/1000;
00276 pstep[2] = tmp(2)/1000;
00277
00278
00279 MatrixXf Kextended(3,4);
00280
00281 Kextended<<K_,Vector3f::Zero(3);
00282
00283 Vector3f projectedPoint=Kextended*tmp;
00284
00285
00286
00287 int u=(int)(projectedPoint(0)/projectedPoint(2));
00288 int v=(int)(projectedPoint(1)/projectedPoint(2));
00289
00290
00291
00292 if ((u>kernel_size_) && (u < (int)rgb_image_->width-kernel_size_) && (v>kernel_size_) && (v<(int)rgb_image_->height-kernel_size_))
00293 {
00294
00295
00296 if (((tmp(2)-zbuffer_threshold_)<Z_buffer_(u,v)) && (Z_buffer_(u,v) !=0 ))
00297 {
00298 int rgb_index = v*rgb_image_->step +u*3;
00299
00300
00301
00302
00303 int32_t rgb_packed = 0;
00304
00305
00306 rgb_packed = (rgb_image_->data[(rgb_index)] << 16) | (rgb_image_->data[(rgb_index)+1] <<8 ) | rgb_image_->data[(rgb_index)+2];
00307 memcpy(&pstep[3], &rgb_packed, sizeof(int32_t));
00308 if (saveFile)
00309 outFile<<"0"<<std::endl;
00310 } else {
00311
00312 pstep[3] = (0xff << 16) | (0x00 << 8) | 0x00;
00313 if (saveFile)
00314 outFile<<"1"<<std::endl;
00315 }
00316 } else {
00317 int32_t rgb_packed = 0;
00318 memcpy(&pstep[3], &rgb_packed, sizeof(int32_t));
00319
00320 if (saveFile) outFile<<"0"<<std::endl;
00321 }
00322 }
00323 }
00324
00325 this->cloud2_raw_publisher_.publish(this->PointCloud2_msg_);
00326
00327
00328 }
00329
00330
00331
00332 rgb_sent_ = true;
00333 depth_sent_ = true;
00334 intens_sent_ = true;
00335
00336 if (saveFile) {
00337 outFile.close();
00338 sensor_msgs::CvBridge g_bridge;
00339 boost::format g_format;
00340 g_format.parse("outRGB%04i.%s");
00341 if (g_bridge.fromImage(*rgb_image_, "bgr8")) {
00342 IplImage *image = g_bridge.toIpl();
00343 if (image) {
00344 std::string filename = (g_format % saveFileCounter % "png").str();
00345 cvSaveImage(filename.c_str(), image);
00346 ROS_INFO("Saved image %s", filename.c_str());
00347
00348 } else {
00349 ROS_WARN("Couldn't save image, no data!");
00350 }
00351 } else {
00352 ROS_ERROR("Unable to convert %s image to bgr8", rgb_image_->encoding.c_str());
00353 }
00354
00355
00356 g_format.parse("outINTENS%04i.%s");
00357
00358 if (g_bridge.fromImage(*intens_image_, "bgr8")) {
00359 IplImage *image = g_bridge.toIpl();
00360 if (image) {
00361 std::string filename = (g_format % saveFileCounter % "png").str();
00362 cvSaveImage(filename.c_str(), image);
00363 ROS_INFO("Saved image %s", filename.c_str());
00364 } else {
00365 ROS_WARN("Couldn't save image, no data!");
00366 }
00367 }
00368 else {
00369 ROS_ERROR("Unable to convert %s image to bgr8", rgb_image_->encoding.c_str());
00370 }
00371
00372 saveFile = false;
00373 }
00374 }
00375 }
00376
00377 bool ZyonzTofColorDriverNode::saveImageCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
00378 {
00379
00380
00381
00382 saveFileCounter++;
00383 fileName="outCamCube";
00384 std::stringstream ss;
00385 ss << saveFileCounter;
00386 fileName+=ss.str();
00387 fileName+=".m";
00388
00389 outFile.open(fileName.c_str());
00390 if( !outFile ) {
00391 ROS_INFO("ZyonzTofColorDriverNode::saveImageCallback: Error obrint el fitxer de sortida!");
00392 saveFile=false;
00393 } else {
00394 outFile<<"% X Y Z u v occlusion "<<std::endl;
00395 saveFile=true;
00396 }
00397
00398 ROS_INFO("ZyonzTofColorDriverNode::saveImageCallback: New Request Received! %s", fileName.c_str());
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421 return true;
00422 }
00423
00424
00425
00426
00427
00428 void ZyonzTofColorDriverNode::postNodeOpenHook(void)
00429 {
00430 }
00431
00432 void ZyonzTofColorDriverNode::addNodeDiagnostics(void)
00433 {
00434 }
00435
00436 void ZyonzTofColorDriverNode::addNodeOpenedTests(void)
00437 {
00438 }
00439
00440 void ZyonzTofColorDriverNode::addNodeStoppedTests(void)
00441 {
00442 }
00443
00444 void ZyonzTofColorDriverNode::addNodeRunningTests(void)
00445 {
00446 }
00447
00448 void ZyonzTofColorDriverNode::reconfigureNodeHook(int level)
00449 {
00450 }
00451
00452 ZyonzTofColorDriverNode::~ZyonzTofColorDriverNode()
00453 {
00454
00455 }
00456
00457
00458 int main(int argc,char *argv[])
00459 {
00460 return driver_base::main<ZyonzTofColorDriverNode>(argc,argv,"zyonz_tof_color_driver");
00461 }