optris_imager_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012/2013
00005  *  Nuremberg Institute of Technology Georg Simon Ohm
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Nuremberg Institute of Technology
00019  *     Georg Simon Ohm nor the authors names may be used to endorse
00020  *     or promote products derived from this software without specific
00021  *     prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Stefan May
00037  *********************************************************************/
00038 
00039 #include "ros/ros.h"
00040 #include <image_transport/image_transport.h>
00041 #include "std_msgs/Float32.h"
00042 #include "std_srvs/Empty.h"
00043 #include "optris_drivers/AutoFlag.h"
00044 
00045 #include "PIImager.h"
00046 #include "ImageBuilder.h"
00047 
00048 #include <sys/stat.h>
00049 using namespace std;
00050 sensor_msgs::Image _thermal_image;
00051 sensor_msgs::Image _visible_image;
00052 std_msgs::Float32 _flag_temperature;
00053 std_msgs::Float32 _box_temperature;
00054 std_msgs::Float32 _chip_temperature;
00055 
00056 image_transport::Publisher* _thermal_pub;
00057 image_transport::Publisher* _visible_pub;
00058 ros::Publisher _flag_pub;
00059 ros::Publisher _box_pub;
00060 ros::Publisher _chip_pub;
00061 
00062 unsigned int _img_cnt = 0;
00063 optris::PIImager* _imager;
00064 
00071 void onThermalFrame(unsigned short* image, unsigned int w, unsigned int h)
00072 {
00073   memcpy(&_thermal_image.data[0], image, w * h * sizeof(*image));
00074 
00075   _thermal_image.header.seq = ++_img_cnt;
00076   _thermal_image.header.stamp = ros::Time::now();
00077   _thermal_pub->publish(_thermal_image);
00078 
00079   _flag_temperature.data = _imager->getTempFlag();
00080   _box_temperature.data = _imager->getTempBox();
00081   _chip_temperature.data = _imager->getTempChip();
00082   _flag_pub.publish(_flag_temperature);
00083   _box_pub.publish(_box_temperature);
00084   _chip_pub.publish(_chip_temperature);
00085 }
00086 
00087 void onVisibleFrame(unsigned char* image, unsigned int w, unsigned int h)
00088 {
00089   memcpy(&_visible_image.data[0], image, 2 * w * h * sizeof(*image));
00090 
00091   _visible_image.header.seq = _img_cnt;
00092   _visible_image.header.stamp = ros::Time::now();
00093   _visible_pub->publish(_visible_image);
00094 }
00095 
00096 bool onAutoFlag(optris_drivers::AutoFlag::Request &req, optris_drivers::AutoFlag::Response &res)
00097 {
00098   _imager->setAutoFlag(req.autoFlag);
00099   return true;
00100 }
00101 
00102 bool onForceFlag(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
00103 {
00104   _imager->forceFlagEvent();
00105   return true;
00106 }
00107 
00108 int main(int argc, char **argv)
00109 {
00110   ros::init(argc, argv, "optris_imager_node");
00111 
00112   // private node handle to support command line parameters for rosrun
00113   ros::NodeHandle n_("~");
00114 
00115   std::string xmlConfig = "";
00116   n_.getParam("xmlConfig", xmlConfig);
00117 
00118   // A specific configuration file for each imager device is needed (cf. config directory)
00119   struct stat s;
00120   if(stat(xmlConfig.c_str(), &s) != 0)
00121   {
00122     std::cerr << "usage: rosrun <package> <node> _xmlConfig:=<xmlConfig>" << std::endl;
00123     std::cerr << " verify that <xmlConfig> exists" << std::endl;
00124     return -1;
00125   }
00126 
00127   ros::NodeHandle n;
00128 
00129   _imager = new optris::PIImager(xmlConfig.c_str());
00130 
00131   unsigned char* bufferRaw = new unsigned char[_imager->getRawBufferSize()];
00132 
00133   image_transport::ImageTransport it(n);
00134 
00135   _imager->setFrameCallback(onThermalFrame);
00136 
00137   image_transport::Publisher tpub = it.advertise("thermal_image", 1);
00138   _thermal_pub = &tpub;
00139 
00140   _thermal_image.header.frame_id = "thermal_image";
00141   _thermal_image.height = _imager->getHeight();
00142   _thermal_image.width = _imager->getWidth();
00143   _thermal_image.encoding = "mono16";
00144   _thermal_image.step = _thermal_image.width * 2;
00145   _thermal_image.data.resize(_thermal_image.height * _thermal_image.step);
00146 
00147   image_transport::Publisher vpub;
00148 
00149   if(_imager->hasBispectralTechnology())
00150   {
00151     _imager->setVisibleFrameCallback(onVisibleFrame);
00152 
00153     vpub = it.advertise("visible_image", 1);
00154     _visible_pub = &vpub;
00155 
00156     _visible_image.header.frame_id = "visible_image";
00157     _visible_image.height = _imager->getVisibleHeight();
00158     _visible_image.width = _imager->getVisibleWidth();
00159     _visible_image.encoding = "yuv422";
00160     _visible_image.step = _visible_image.width * 2;
00161     _visible_image.data.resize(_visible_image.height * _visible_image.step);
00162   }
00163 
00164   ros::ServiceServer sAuto = n_.advertiseService("auto_flag", onAutoFlag);
00165   ros::ServiceServer sForce = n_.advertiseService("force_flag", onForceFlag);
00166 
00167   _flag_pub = n.advertise < std_msgs::Float32 > ("temperature_flag", 1);
00168   _box_pub = n.advertise < std_msgs::Float32 > ("temperature_box", 1);
00169   _chip_pub = n.advertise < std_msgs::Float32 > ("temperature_chip", 1);
00170 
00171   _imager->startStreaming();
00172 
00173   // loop over acquire-process-release-publish steps
00174   // Images are published in raw temperature format (unsigned short, see onFrame callback for details)
00175   ros::Rate loop_rate(_imager->getMaxFramerate());
00176   while(ros::ok())
00177   {
00178     _imager->getFrame(bufferRaw);
00179     _imager->process(bufferRaw);
00180     _imager->releaseFrame();
00181     ros::spinOnce();
00182     loop_rate.sleep();
00183   }
00184 
00185   delete[] bufferRaw;
00186   delete _imager;
00187 
00188   return 0;
00189 }


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 Mon Oct 6 2014 03:09:39