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 "libirimager/ImageBuilder.h"
00043
00044 #include <camera_info_manager/camera_info_manager.h>
00045 #include <sensor_msgs/CameraInfo.h>
00046
00047 unsigned char* _bufferThermal = NULL;
00048 unsigned char* _bufferVisible = NULL;
00049 image_transport::Publisher* _pubThermal;
00050 image_transport::Publisher* _pubVisible;
00051 unsigned int _frame = 0;
00052
00053 optris::ImageBuilder _iBuilder;
00054 optris::EnumOptrisColoringPalette _palette;
00055
00056 sensor_msgs::CameraInfo _camera_info;
00057 image_transport::CameraPublisher* _camera_info_pub = NULL;
00058 camera_info_manager::CameraInfoManager* _camera_info_manager = NULL;
00059
00060 void onThermalDataReceive(const sensor_msgs::ImageConstPtr& image)
00061 {
00062
00063 if((_pubThermal->getNumSubscribers() == 0) && (_camera_info_pub->getNumSubscribers() == 0))
00064 return;
00065
00066 unsigned short* data = (unsigned short*)&image->data[0];
00067 _iBuilder.setData(image->width, image->height, data);
00068
00069 if(_bufferThermal==NULL)
00070 _bufferThermal = new unsigned char[image->width * image->height * 3];
00071
00072 _iBuilder.convertTemperatureToPaletteImage(_bufferThermal, true);
00073
00074 sensor_msgs::Image img;
00075 img.header.frame_id = "thermal_image_view";
00076 img.height = image->height;
00077 img.width = image->width;
00078 img.encoding = "rgb8";
00079 img.step = image->width*3;
00080 img.data.resize(img.height*img.step);
00081 img.header.seq = ++_frame;
00082 img.header.stamp = ros::Time::now();
00083
00084 for(unsigned int i=0; i<image->width*image->height*3; i++) {
00085 img.data[i] = _bufferThermal[i];
00086 }
00087
00088 _camera_info = _camera_info_manager->getCameraInfo();
00089 _camera_info.header = img.header;
00090 _camera_info_pub->publish(img, _camera_info);
00091
00092 _pubThermal->publish(img);
00093 }
00094
00095 void onVisibleDataReceive(const sensor_msgs::ImageConstPtr& image)
00096 {
00097
00098 if(_pubVisible->getNumSubscribers() == 0)
00099 return;
00100
00101 if(_bufferVisible==NULL)
00102 _bufferVisible = new unsigned char[image->width * image->height * 3];
00103
00104 const unsigned char* data = &image->data[0];
00105 _iBuilder.yuv422torgb24(data, _bufferVisible, image->width, image->height);
00106
00107 sensor_msgs::Image img;
00108 img.header.frame_id = "visible_image_view";
00109 img.height = image->height;
00110 img.width = image->width;
00111 img.encoding = "rgb8";
00112 img.step = image->width*3;
00113 img.data.resize(img.height*img.step);
00114
00115 img.header.seq = _frame;
00116 img.header.stamp = ros::Time::now();
00117
00118 for(unsigned int i=0; i<image->width*image->height*3; i++) {
00119 img.data[i] = _bufferVisible[i];
00120 }
00121
00122 _pubVisible->publish(img);
00123 }
00124
00125 int main (int argc, char* argv[])
00126 {
00127 ros::init (argc, argv, "optris_colorconvert_node");
00128
00129
00130 ros::NodeHandle n_("~");
00131
00132 int palette = 6;
00133 n_.getParam("palette", palette);
00134 _palette = (optris::EnumOptrisColoringPalette) palette;
00135
00136 optris::EnumOptrisPaletteScalingMethod scalingMethod = optris::eMinMax;
00137 int sm;
00138 n_.getParam("paletteScaling", sm);
00139 if(sm>=1 && sm <=4) scalingMethod = (optris::EnumOptrisPaletteScalingMethod) sm;
00140
00141 _iBuilder.setPaletteScalingMethod(scalingMethod);
00142 _iBuilder.setPalette(_palette);
00143
00144 double tMin = 20.;
00145 double tMax = 40.;
00146 double looprate = 30.;
00147
00148 n_.getParam("temperatureMin", tMin);
00149 n_.getParam("temperatureMax", tMax);
00150 n_.getParam("looprate", looprate);
00151
00152 _iBuilder.setManualTemperatureRange((float)tMin, (float)tMax);
00153
00154 ros::NodeHandle n;
00155 image_transport::ImageTransport it(n);
00156 image_transport::Subscriber subThermal = it.subscribe("thermal_image", 1, onThermalDataReceive);
00157 image_transport::Subscriber subVisible = it.subscribe("visible_image", 1, onVisibleDataReceive);
00158
00159 image_transport::Publisher pubt = it.advertise("thermal_image_view", 1);
00160 image_transport::Publisher pubv = it.advertise("visible_image_view", 1);
00161
00162 _pubThermal = &pubt;
00163 _pubVisible = &pubv;
00164
00165 std::string camera_name;
00166 std::string camera_info_url;
00167 n_.getParam("camera_name", camera_name);
00168 n_.getParam("camera_info_url", camera_info_url);
00169
00170
00171
00172 camera_info_manager::CameraInfoManager cinfo_manager(n);
00173 _camera_info_manager = &cinfo_manager;
00174
00175 if (!_camera_info_manager->setCameraName(camera_name))
00176 {
00177
00178
00179 ROS_WARN_STREAM("[" << camera_name
00180 << "] name not valid"
00181 << " for camera_info_manger");
00182 }
00183
00184 if (_camera_info_manager->validateURL(camera_info_url))
00185 {
00186 if ( !_camera_info_manager->loadCameraInfo(camera_info_url) )
00187 {
00188 ROS_WARN( "camera_info_url does not contain calibration data." );
00189 }
00190 else if ( !_camera_info_manager->isCalibrated() )
00191 {
00192 ROS_WARN( "Camera is not calibrated. Using default values." );
00193 }
00194 }
00195 else
00196 {
00197 ROS_ERROR_STREAM_ONCE( "Calibration URL syntax is not supported by CameraInfoManager." );
00198 }
00199
00200
00201 image_transport::CameraPublisher cinfo_pub = it.advertiseCamera("image_raw", 1);
00202 _camera_info_pub = &cinfo_pub;
00203
00204
00205 std::string key;
00206 if(ros::param::search("thermal_image/compressed/format", key))
00207 {
00208 ros::param::set(key, "png");
00209 }
00210 if(ros::param::search("thermal_image/compressed/png_level", key))
00211 {
00212 ros::param::set(key, 9);
00213 }
00214
00215
00216 ros::Rate loop_rate(looprate);
00217 while (ros::ok())
00218 {
00219 ros::spinOnce();
00220 loop_rate.sleep();
00221 }
00222
00223 if(_bufferThermal) delete [] _bufferThermal;
00224 if(_bufferVisible) delete [] _bufferVisible;
00225 }
optris_drivers
Author(s): Stefan May (Nuremberg Institute of Technology Georg Simon Ohm - www.th-nuernberg.de), 64-Bit platform supported by Fraunhofer IPA (www.ipa.fraunhofer.de), Support for ROS hydro migration by Christopher-Eyk Hrabia (DAI-Labor, Technische Universität Berlin)
autogenerated on Wed Aug 26 2015 15:11:37