flir.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* author: Radu Bogdan Rusu <rusu@cs.tum.edu> */
00031 
00032 #include "flir.h"
00033 
00035 #define FLIR_EXCEPT(except, msg) \
00036   { \
00037     char buf[100]; \
00038     snprintf(buf, 100, "[FLIR::%s]: " msg, __FUNCTION__); \
00039     throw except(buf); \
00040   }
00041 
00043 #define FLIR_EXCEPT_ARGS(except, msg, ...) \
00044   { \
00045     char buf[100]; \
00046     snprintf(buf, 100, "[FLIR::%s]: " msg, __FUNCTION__, __VA_ARGS__); \
00047     throw except(buf); \
00048   }
00049 
00050 unicap::FLIR::FLIR () : device_id_(1), color_space_(0), video_format_(0), debug_(false)
00051 {
00052 }
00053 
00054 unicap::FLIR::~FLIR ()
00055 {
00056 }
00057 
00059 // Set up the device
00060 int
00061   unicap::FLIR::open ()
00062 {
00063   // Device ID number
00064   if (!SUCCESS (unicap_enumerate_devices (NULL, &device_, device_id_)))
00065     FLIR_EXCEPT_ARGS(unicap::Exception, "Could not get info for device %i!", device_id_);
00066 
00067   if ( !SUCCESS (unicap_open (&handle_, &device_)) )
00068   {
00069     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to open camera port %s!", device_.identifier);
00070     return (-1);
00071   }
00072 
00073   // Set the desired format
00074   unicap_void_format (&format_spec_);
00075 //  format_spec.fourcc = FOURCC('U','Y','V','Y');
00076 
00077   // Get the list of video formats of the given colorformat
00078   if (debug_)
00079   {
00080     fprintf (stderr, "[CompositeNode::FLIR] Available color spaces:");
00081     for ( int i = 0; SUCCESS (unicap_enumerate_formats (handle_, &format_spec_, &format_, i)); i++)
00082       fprintf (stderr, " %d(%s) ", i, format_.identifier);
00083     fprintf (stderr, "\n");
00084   }
00085 
00086   if (!SUCCESS (unicap_enumerate_formats (handle_, &format_spec_, &format_, color_space_) ) )
00087   {
00088     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to set color space to %d!", color_space_);
00089     return (-1);
00090   }
00091 
00092   // If a video format has more than one size, ask for which size to use
00093   if (format_.size_count)
00094   {
00095     if (debug_)
00096     {
00097       fprintf (stderr, "[CompositeNode::FLIR] Available video formats:");
00098        for (int i = 0; i < format_.size_count; i++)
00099          fprintf (stderr, " %d(%dx%d) ", i, format_.sizes[i].width, format_.sizes[i].height);
00100       fprintf (stderr, "\n");
00101     }
00102     format_.size.width = format_.sizes[video_format_].width;
00103     format_.size.height = format_.sizes[video_format_].height;
00104   }
00105 
00106   if (!SUCCESS (unicap_set_format (handle_, &format_) ) )
00107   {
00108     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to set video format to %d!", video_format_);
00109     return (-1);
00110   }
00111 
00112   // Start the capture process on the device
00113   if (!SUCCESS (unicap_start_capture (handle_) ) )
00114   {
00115     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to capture on device: %s!", device_.identifier);
00116     return (-1);
00117   }
00118 
00119   // Initialize the image buffer
00120   memset (&buffer_,   0, sizeof (unicap_data_buffer_t));
00121 
00122   // Allocate buffer data
00123   buffer_.data = (unsigned char*)(malloc (format_.size.width * format_.size.height * format_.bpp / 8));
00124   buffer_.buffer_size = format_.size.width * format_.size.height * format_.bpp / 8;
00125 
00126   return (0);
00127 }
00128 
00130 // Shutdown the device
00131 int
00132   unicap::FLIR::close ()
00133 {
00134   // Stop the device
00135   if ( !SUCCESS (unicap_stop_capture (handle_) ) )
00136     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to stop capture on device: %s!", device_.identifier);
00137 
00138   free (buffer_.data);
00139 
00140   // Close the device
00141   if ( !SUCCESS (unicap_close (handle_) ) )
00142   {
00143     FLIR_EXCEPT_ARGS(unicap::Exception, "Failed to close device: %s!", device_.identifier);
00144     return (-1);
00145   }
00146 
00147   return (0);
00148 }
00149 
00151 // Read data from the device
00152 void
00153 //  unicap::FLIR::readData (camera_data_t* data)
00154   unicap::FLIR::readData (sensor_msgs::Image &image)
00155 {
00156   // Queue the buffer
00157   // The buffer now gets filled with image data by the capture device
00158   if (!SUCCESS (unicap_queue_buffer (handle_, &buffer_) ) )
00159     return;
00160 
00161   // Wait until the image buffer is ready
00162   if (!SUCCESS (unicap_wait_buffer (handle_, &returned_buffer_) ) ) {}
00163 //    return;
00164 
00165   // To do: implement the code for different formats later
00166   image.width  = buffer_.format.size.width;
00167   image.height = buffer_.format.size.height;
00168   image.encoding = "raw";
00169   //  image.colorspace  = "mono16";
00170   // image.label       = "flir-thermal";
00171   //  image.set_data_size (buffer_.buffer_size);
00172   memcpy (&(image.data[0]), buffer_.data, buffer_.buffer_size);  
00173 
00174   return;
00175 }


flir_driver
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu)
autogenerated on Mon Oct 6 2014 09:21:48