Skip to content

Commit e2ee6bc

Browse files
k-okadak-okada
committed
color_filter_nodelet.cpp : use PointCloud2 instead of PointCloud
1 parent 9bcd45d commit e2ee6bc

File tree

1 file changed

+72
-61
lines changed

1 file changed

+72
-61
lines changed

‎src/nodelet/color_filter_nodelet.cpp‎

Lines changed: 72 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
#include "opencv_apps/nodelet.h"
3737
#include <image_transport/image_transport.h>
3838
#include <sensor_msgs/image_encodings.h>
39-
#include <sensor_msgs/PointCloud.h>
39+
#include <sensor_msgs/PointCloud2.h>
4040
#include <cv_bridge/cv_bridge.h>
4141

4242
#include <opencv2/highgui/highgui.hpp>
@@ -79,7 +79,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
7979
boost::mutex mutex_;
8080

8181
// publisch color space
82-
sensor_msgs::PointCloud color_space_msg_;
82+
sensor_msgs::PointCloud2 color_space_msg_;
8383
ros::Publisher color_space_pub_;
8484

8585
virtual void reconfigureCallback(Config &new_config, uint32_t level) = 0;
@@ -178,7 +178,25 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
178178
img_pub_ = advertiseImage(*pnh_, "image", 1);
179179

180180
// to advertise you can do it like this (as above):
181-
color_space_pub_ = pnh_->advertise<sensor_msgs::PointCloud>("color_space", 1);
181+
color_space_pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("color_space", 1);
182+
color_space_msg_.header.frame_id = "/map";
183+
color_space_msg_.fields.resize(4);
184+
color_space_msg_.fields[0].name = "x";
185+
color_space_msg_.fields[0].offset = 0;
186+
color_space_msg_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
187+
color_space_msg_.fields[0].count = 1;
188+
color_space_msg_.fields[1].name = "y";
189+
color_space_msg_.fields[1].offset = 4;
190+
color_space_msg_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
191+
color_space_msg_.fields[1].count = 1;
192+
color_space_msg_.fields[2].name = "z";
193+
color_space_msg_.fields[2].offset = 8;
194+
color_space_msg_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
195+
color_space_msg_.fields[2].count = 1;
196+
color_space_msg_.fields[3].name = "rgb";
197+
color_space_msg_.fields[3].offset = 12;
198+
color_space_msg_.fields[3].datatype = sensor_msgs::PointField::UINT32;
199+
color_space_msg_.fields[3].count = 1;
182200

183201
onInitPostProcess();
184202
}
@@ -220,27 +238,25 @@ class RGBColorFilterNodelet
220238
output_image);
221239

222240
/// publish color spaces
223-
color_space_msg_.header.frame_id = "/map";
224-
if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
225-
color_space_msg_.points.resize(input_image.rows*input_image.cols);
226-
color_space_msg_.channels.resize(3);
227-
color_space_msg_.channels[0].name = "b";
228-
color_space_msg_.channels[1].name = "g";
229-
color_space_msg_.channels[2].name = "r";
230-
color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
231-
color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
232-
color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
241+
if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
242+
color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
243+
color_space_msg_.width = input_image.cols;
244+
color_space_msg_.height = input_image.rows;
245+
color_space_msg_.point_step = 16;
246+
color_space_msg_.row_step = color_space_msg_.width;
233247
}
234248
for ( size_t i = 0; i < input_image.rows * input_image.cols; i++ ) {
235-
float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
236-
float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
237-
float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
238-
color_space_msg_.points[i].x = r;
239-
color_space_msg_.points[i].y = g;
240-
color_space_msg_.points[i].z = b;
241-
color_space_msg_.channels[0].values[i] = r;
242-
color_space_msg_.channels[1].values[i] = g;
243-
color_space_msg_.channels[2].values[i] = b;
249+
unsigned char r = input_image.at<cv::Vec3b>(i)[0];
250+
unsigned char g = input_image.at<cv::Vec3b>(i)[1];
251+
unsigned char b = input_image.at<cv::Vec3b>(i)[2];
252+
float x = r/255.0;
253+
float y = g/255.0;
254+
float z = b/255.0;
255+
memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
256+
memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
257+
memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
258+
unsigned char rgb[4] = {r, g, b, 0};
259+
memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
244260
}
245261
color_space_pub_.publish(color_space_msg_);
246262
}
@@ -306,30 +322,28 @@ class HLSColorFilterNodelet
306322
}
307323

308324
/// publish color spaces
309-
color_space_msg_.header.frame_id = "/map";
310-
if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
311-
color_space_msg_.points.resize(input_image.rows*input_image.cols);
312-
color_space_msg_.channels.resize(3);
313-
color_space_msg_.channels[0].name = "b";
314-
color_space_msg_.channels[1].name = "g";
315-
color_space_msg_.channels[2].name = "r";
316-
color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
317-
color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
318-
color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
325+
if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
326+
color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
327+
color_space_msg_.width = input_image.cols;
328+
color_space_msg_.height = input_image.rows;
329+
color_space_msg_.point_step = 16;
330+
color_space_msg_.row_step = color_space_msg_.width;
319331
}
320332
for ( size_t i = 0; i < input_image.rows * input_image.cols; i++ ) {
333+
unsigned char r = input_image.at<cv::Vec3b>(i)[0];
334+
unsigned char g = input_image.at<cv::Vec3b>(i)[1];
335+
unsigned char b = input_image.at<cv::Vec3b>(i)[2];
321336
float h = hls_image.at<cv::Vec3b>(i)[0]*2;
322337
float l = hls_image.at<cv::Vec3b>(i)[1]/255.0;
323338
float s = hls_image.at<cv::Vec3b>(i)[2]/255.0;
324-
float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
325-
float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
326-
float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
327-
color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
328-
color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
329-
color_space_msg_.points[i].z = l;
330-
color_space_msg_.channels[0].values[i] = r;
331-
color_space_msg_.channels[1].values[i] = g;
332-
color_space_msg_.channels[2].values[i] = b;
339+
float x = s*cos(h*M_PI/180.0);
340+
float y = s*sin(h*M_PI/180.0);
341+
float z = l;
342+
memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
343+
memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
344+
memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
345+
unsigned char rgb[4] = {r, g, b, 0};
346+
memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
333347
}
334348
color_space_pub_.publish(color_space_msg_);
335349
}
@@ -393,32 +407,29 @@ class HSVColorFilterNodelet
393407
cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
394408
output_image = output_image_0 | output_image_360;
395409
}
396-
397410
/// publish color spaces
398-
color_space_msg_.header.frame_id = "/map";
399-
if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
400-
color_space_msg_.points.resize(input_image.rows*input_image.cols);
401-
color_space_msg_.channels.resize(3);
402-
color_space_msg_.channels[0].name = "b";
403-
color_space_msg_.channels[1].name = "g";
404-
color_space_msg_.channels[2].name = "r";
405-
color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
406-
color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
407-
color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
411+
if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
412+
color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
413+
color_space_msg_.width = input_image.cols;
414+
color_space_msg_.height = input_image.rows;
415+
color_space_msg_.point_step = 16;
416+
color_space_msg_.row_step = color_space_msg_.width;
408417
}
409418
for ( size_t i = 0; i < input_image.rows * input_image.cols; i++ ) {
419+
unsigned char r = input_image.at<cv::Vec3b>(i)[0];
420+
unsigned char g = input_image.at<cv::Vec3b>(i)[1];
421+
unsigned char b = input_image.at<cv::Vec3b>(i)[2];
410422
float h = hsv_image.at<cv::Vec3b>(i)[0]*2;
411423
float s = hsv_image.at<cv::Vec3b>(i)[1]/255.0;
412424
float v = hsv_image.at<cv::Vec3b>(i)[2]/255.0;
413-
float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
414-
float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
415-
float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
416-
color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
417-
color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
418-
color_space_msg_.points[i].z = v;
419-
color_space_msg_.channels[0].values[i] = r;
420-
color_space_msg_.channels[1].values[i] = g;
421-
color_space_msg_.channels[2].values[i] = b;
425+
float x = s*cos(h*M_PI/180.0);
426+
float y = s*sin(h*M_PI/180.0);
427+
float z = v;
428+
memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
429+
memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
430+
memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
431+
unsigned char rgb[4] = {r, g, b, 0};
432+
memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
422433
}
423434
color_space_pub_.publish(color_space_msg_);
424435
}

0 commit comments

Comments
 (0)