Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
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 #include "ros/ros.h"
00040 #include <image_transport/image_transport.h>
00041
00042 #include "ImageBuilder.h"
00043
00044 unsigned char* _bufferThermal = NULL;
00045 unsigned char* _bufferVisible = NULL;
00046 image_transport::Publisher* _pubThermal;
00047 image_transport::Publisher* _pubVisible;
00048 unsigned int _frame = 0;
00049
00050 optris::ImageBuilder _iBuilder;
00051 optris::EnumOptrisColoringPalette _palette;
00052
00053 void onThermalDataReceive(const sensor_msgs::ImageConstPtr& image)
00054 {
00055 unsigned short* data = (unsigned short*)&image->data[0];
00056 _iBuilder.setData(image->width, image->height, data);
00057
00058 if(_bufferThermal==NULL)
00059 _bufferThermal = new unsigned char[image->width * image->height * 3];
00060
00061 _iBuilder.convertTemperatureToPaletteImage(_bufferThermal, true);
00062
00063 sensor_msgs::Image img;
00064 img.header.frame_id = "thermal_image_view";
00065 img.height = image->height;
00066 img.width = image->width;
00067 img.encoding = "rgb8";
00068 img.step = image->width*3;
00069 img.data.resize(img.height*img.step);
00070 img.header.seq = ++_frame;
00071 img.header.stamp = ros::Time::now();
00072
00073 for(unsigned int i=0; i<image->width*image->height*3; i++)
00074 {
00075 img.data[i] = _bufferThermal[i];
00076 }
00077
00078 _pubThermal->publish(img);
00079 }
00080
00081 void onVisibleDataReceive(const sensor_msgs::ImageConstPtr& image)
00082 {
00083 if(_bufferVisible==NULL)
00084 _bufferVisible = new unsigned char[image->width * image->height * 3];
00085
00086 const unsigned char* data = &image->data[0];
00087 _iBuilder.yuv422torgb24(data, _bufferVisible, image->width, image->height);
00088
00089 sensor_msgs::Image img;
00090 img.header.frame_id = "visible_image_view";
00091 img.height = image->height;
00092 img.width = image->width;
00093 img.encoding = "rgb8";
00094 img.step = image->width*3;
00095 img.data.resize(img.height*img.step);
00096
00097 img.header.seq = _frame;
00098 img.header.stamp = ros::Time::now();
00099
00100 for(unsigned int i=0; i<image->width*image->height*3; i++)
00101 {
00102 img.data[i] = _bufferVisible[i];
00103 }
00104
00105 _pubVisible->publish(img);
00106 }
00107
00108 int main (int argc, char* argv[])
00109 {
00110 ros::init (argc, argv, "optris_colorconvert_node");
00111
00112
00113 ros::NodeHandle n_("~");
00114
00115 int palette = 6;
00116 n_.getParam("palette", palette);
00117 _palette = (optris::EnumOptrisColoringPalette) palette;
00118
00119 optris::EnumOptrisPaletteScalingMethod scalingMethod = optris::eMinMax;
00120 int sm;
00121 n_.getParam("paletteScaling", sm);
00122 if(sm>=1 && sm <=4) scalingMethod = (optris::EnumOptrisPaletteScalingMethod) sm;
00123
00124 _iBuilder.setPaletteScalingMethod(scalingMethod);
00125 _iBuilder.setPalette(_palette);
00126
00127 double tMin = 20.;
00128 double tMax = 40.;
00129 n_.getParam("temperatureMin", tMin);
00130 n_.getParam("temperatureMax", tMax);
00131 _iBuilder.setManualTemperatureRange((float)tMin, (float)tMax);
00132
00133 ros::NodeHandle n;
00134 image_transport::ImageTransport it(n);
00135 image_transport::Subscriber subThermal = it.subscribe("thermal_image", 1, onThermalDataReceive);
00136 image_transport::Subscriber subVisible = it.subscribe("visible_image", 1, onVisibleDataReceive);
00137
00138 image_transport::Publisher pubt = it.advertise("thermal_image_view", 1);
00139 image_transport::Publisher pubv = it.advertise("visible_image_view", 1);
00140 _pubThermal = &pubt;
00141 _pubVisible = &pubv;
00142
00143
00144 ros::Rate loop_rate(30);
00145 while (ros::ok())
00146 {
00147 ros::spinOnce();
00148 loop_rate.sleep();
00149 }
00150
00151 if(_bufferThermal) delete [] _bufferThermal;
00152 if(_bufferVisible) delete [] _bufferVisible;
00153 }