optris_imager_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012-2015
00005  *  Technische Hochschule Nürnberg 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 <sensor_msgs/TimeReference.h>
00042 #include "std_msgs/Float32.h"
00043 #include "std_srvs/Empty.h"
00044 #include "optris_drivers/AutoFlag.h"
00045 #include "optris_drivers/SwitchTemperatureRange.h"
00046 #include <optris_drivers/Temperature.h>
00047 
00048 #include "libirimager/IRImager.h"
00049 #include "libirimager/ImageBuilder.h"
00050 
00051 #include <sys/stat.h>
00052 using namespace std;
00053 
00054 sensor_msgs::Image _thermal_image;
00055 sensor_msgs::Image _visible_image;
00056 sensor_msgs::TimeReference _optris_timer;
00057 optris_drivers::Temperature _internal_temperature;
00058 
00059 image_transport::Publisher* _thermal_pub = NULL;
00060 image_transport::Publisher* _visible_pub = NULL;
00061 
00062 ros::Publisher _timer_pub;
00063 ros::Publisher _temp_pub;
00064 
00065 unsigned int _img_cnt = 0;
00066 optris::IRImager* _imager;
00067 
00076 void onThermalFrame(unsigned short* image, unsigned int w, unsigned int h, long long timestamp, void* arg)
00077 {
00078   memcpy(&_thermal_image.data[0], image, w * h * sizeof(*image));
00079 
00080   _thermal_image.header.seq = ++_img_cnt;
00081   _thermal_image.header.stamp = ros::Time::now();
00082   _thermal_pub->publish(_thermal_image);
00083 
00084   _optris_timer.header.seq = _thermal_image.header.seq;
00085   _optris_timer.header.stamp = _thermal_image.header.stamp;
00086   _optris_timer.time_ref.fromNSec(timestamp);
00087 
00088   _internal_temperature.header.seq=_thermal_image.header.seq;
00089   _internal_temperature.header.stamp = _thermal_image.header.stamp;
00090 
00091   _internal_temperature.temperature_flag = _imager->getTempFlag();
00092   _internal_temperature.temperature_box = _imager->getTempBox();
00093   _internal_temperature.temperature_chip = _imager->getTempChip();
00094 
00095   _timer_pub.publish(_optris_timer);
00096   _temp_pub.publish(_internal_temperature);
00097 }
00098 
00107 void onVisibleFrame(unsigned char* image, unsigned int w, unsigned int h, long long timestamp, void* arg)
00108 {
00109   if(_visible_pub->getNumSubscribers()==0) return;
00110 
00111   memcpy(&_visible_image.data[0], image, 2 * w * h * sizeof(*image));
00112 
00113   _visible_image.header.seq   = _img_cnt;
00114   _visible_image.header.stamp = ros::Time::now();
00115   _visible_pub->publish(_visible_image);
00116 }
00117 
00118 bool onAutoFlag(optris_drivers::AutoFlag::Request &req, optris_drivers::AutoFlag::Response &res)
00119 {
00120   _imager->setAutoFlag(req.autoFlag);
00121   res.isAutoFlagActive = _imager->getAutoFlag();
00122   return true;
00123 }
00124 
00125 bool onForceFlag(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
00126 {
00127   _imager->forceFlagEvent();
00128   return true;
00129 }
00130 
00131 bool onSwitchTemperatureRange(optris_drivers::SwitchTemperatureRange::Request &req, optris_drivers::SwitchTemperatureRange::Response &res)
00132 {
00133   bool validParam = true;
00134   switch(req.temperatureRange)
00135   {
00136   case 0:
00137     _imager->setTempRange(optris::TM20_100);
00138     break;
00139   case 1:
00140     _imager->setTempRange(optris::T0_250);
00141     break;
00142   case 2:
00143     _imager->setTempRange(optris::T150_900);
00144     break;
00145   default:
00146     std::cerr << "Wrong temperature range parameter passed, valid = [0, 1, 2]" << endl;
00147     validParam = false;
00148     break;
00149   }
00150   if(validParam)
00151   {
00152     _imager->forceFlagEvent(1000.f);
00153     res.success = true;
00154   }
00155   return true;
00156 }
00157 
00158 int main(int argc, char **argv)
00159 {
00160   ros::init(argc, argv, "optris_imager_node");
00161 
00162   // private node handle to support command line parameters for rosrun
00163   ros::NodeHandle n_("~");
00164 
00165   std::string xmlConfig = "";
00166   n_.getParam("xmlConfig", xmlConfig);
00167 
00168   // A specific configuration file for each imager device is needed (cf. config directory)
00169   struct stat s;
00170   if(stat(xmlConfig.c_str(), &s) != 0)
00171   {
00172     std::cerr << "usage: rosrun <package> <node> _xmlConfig:=<xmlConfig>" << std::endl;
00173     std::cerr << " verify that <xmlConfig> exists" << std::endl;
00174     return -1;
00175   }
00176 
00177   ros::NodeHandle n;
00178 
00179   _imager = new optris::IRImager(xmlConfig.c_str());
00180 
00181   unsigned char* bufferRaw = new unsigned char[_imager->getRawBufferSize()];
00182 
00183   image_transport::ImageTransport it(n);
00184 
00185   _imager->setFrameCallback(onThermalFrame);
00186 
00187   image_transport::Publisher tpub = it.advertise("thermal_image", 1);
00188   _thermal_pub = &tpub;
00189 
00190   _thermal_image.header.frame_id = "thermal_image";
00191   _thermal_image.height          = _imager->getHeight();
00192   _thermal_image.width           = _imager->getWidth();
00193   _thermal_image.encoding        = "mono16";
00194   _thermal_image.step            = _thermal_image.width * 2;
00195   _thermal_image.data.resize(_thermal_image.height * _thermal_image.step);
00196 
00197 
00198   image_transport::Publisher vpub;
00199   if(_imager->hasBispectralTechnology())
00200   {
00201     _imager->setVisibleFrameCallback(onVisibleFrame);
00202 
00203     vpub = it.advertise("visible_image", 1);
00204     _visible_pub = &vpub;
00205 
00206     _visible_image.header.frame_id = "visible_image";
00207     _visible_image.height          = _imager->getVisibleHeight();
00208     _visible_image.width           = _imager->getVisibleWidth();
00209     _visible_image.encoding        = "yuv422";
00210     _visible_image.step            = _visible_image.width * 2;
00211     _visible_image.data.resize(_visible_image.height * _visible_image.step);
00212   }
00213 
00214 
00215   // advertise the camera internal timer
00216    _timer_pub= n.advertise<sensor_msgs::TimeReference>("optris_timer", 1 );
00217   _optris_timer.header.frame_id=_thermal_image.header.frame_id;
00218 
00219 
00220   ros::ServiceServer sAuto  = n_.advertiseService("auto_flag",  onAutoFlag);
00221   ros::ServiceServer sForce = n_.advertiseService("force_flag", onForceFlag);
00222   ros::ServiceServer sTemp = n_.advertiseService("switch_temperature_range", onSwitchTemperatureRange);
00223 
00224   // advertise all of the camera's temperatures in a single custom message
00225   _temp_pub = n.advertise <optris_drivers::Temperature> ("internal_temperature", 1);
00226   _internal_temperature.header.frame_id=_thermal_image.header.frame_id;
00227 
00228 
00229   _imager->startStreaming();
00230 
00231   // loop over acquire-process-release-publish steps
00232   // Images are published in raw temperature format (unsigned short, see onFrame callback for details)
00233   ros::Rate loop_rate(_imager->getMaxFramerate());
00234   while(ros::ok())
00235   {
00236     if(_imager->getFrame(bufferRaw)==optris::IRIMAGER_SUCCESS)
00237     {
00238       _imager->process(bufferRaw);
00239       _imager->releaseFrame();
00240     }
00241     ros::spinOnce();
00242     loop_rate.sleep();
00243   }
00244   ros::shutdown();
00245 
00246   delete[] bufferRaw;
00247   delete _imager;
00248 
00249   return 0;
00250 }


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 Thu Jun 6 2019 22:02:23