Go to the documentation of this file.00001 #include <ros/ros.h>
00002
00003 #include <hrl_phri_2011/pcl_basic.h>
00004 #include <hrl_phri_2011/utils.h>
00005 #include <hrl_phri_2011/ForceProcessed.h>
00006 #include <hrl_phri_2011/hsl_rgb_conversions.h>
00007
00008 void colorizeDataPC(const PCRGB& data_pc, PCRGB& color_pc, double saturation=100, double lightness=50)
00009 {
00010 vector<float> data;
00011 for(size_t i=0;i<data_pc.size();i++)
00012 data.push_back(data_pc.points[i].rgb);
00013
00014 float max_val = *std::max_element(data.begin(), data.end());
00015 float min_val = *std::min_element(data.begin(), data.end());
00016 ROS_INFO("Max data value: %f, Min data_value: %f", max_val, min_val);
00017 double h, s, l;
00018 for(size_t i=0;i<data.size();i++) {
00019 PRGB pt;
00020 pt.x = data_pc.points[i].x;
00021 pt.y = data_pc.points[i].y;
00022 pt.z = data_pc.points[i].z;
00023 h = (double) 240.0 * data[i] / max_val;
00024 if(h < 0) h = 0; if(h > 240.0) h = 240.0;
00025 writeHSL(240.0 - h, saturation, lightness, pt.rgb);
00026 color_pc.push_back(pt);
00027 }
00028 }
00029
00030 int main(int argc, char **argv)
00031 {
00032 ros::init(argc, argv, "colorize_data_cloud");
00033 ros::NodeHandle nh;
00034
00035 if(argc < 2 || argc > 4) {
00036 printf("Usage: colorize_data_cloud data_bag output_bag\n");
00037 return 1;
00038 }
00039
00040 vector<PCRGB::Ptr> cloud;
00041 readBagTopic<PCRGB>(argv[1], cloud, "/contact_cloud");
00042 PCRGB color_pc;
00043 colorizeDataPC(*cloud[0], color_pc, 100);
00044 color_pc.header.frame_id = cloud[0]->header.frame_id;
00045
00046 rosbag::Bag bag;
00047 bag.open(argv[2], rosbag::bagmode::Write);
00048 bag.write("/contact_cloud", ros::Time::now(), color_pc);
00049 bag.close();
00050
00051 return 0;
00052 }