swissranger.h
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 #ifndef SWISSRANGER_HH
00033 #define SWISSRANGER_HH
00034 
00035 #include <stdexcept>
00036 #include <stdio.h>
00037 #include <stdlib.h>
00038 #include <string>
00039 #include <string.h>
00040 
00041 #include <opencv/cv.h>
00042 
00043 // For some reason 1.0.10-541 does not provide this
00044 #define DWORD unsigned int
00045 
00046 #define SR_IMG_DISTANCE   0
00047 #define SR_IMG_AMPLITUDE  1
00048 #define SR_IMG_CONFIDENCE 2
00049 
00050 #include <libusbSR.h>
00051 
00052 // ROS include
00053 #include "std_msgs/PointCloud.h"
00054 #include "std_msgs/ImageArray.h"
00055 #include "ros/common.h"
00056 
00057 // Older library: #define MODE (AM_COR_FIX_PTRN | AM_COR_LED_NON_LIN | AM_MEDIAN)
00058 #define MODE (AM_CONF_MAP | AM_COR_FIX_PTRN | AM_SW_ANF | AM_MEDIAN | AM_DENOISE_ANF | AM_MEDIANCROSS | AM_CONV_GRAY)// | AM_SHORT_RANGE)
00059 
00060 namespace swissranger
00061 {
00063   // code borrowed from drivers/laser/hokuyo_driver/hokuyo.h
00064 #define DEF_EXCEPTION(name, parent) \
00065   class name  : public parent { \
00066   public: \
00067     name (const char* msg) : parent (msg) {} \
00068   }
00069 
00071   DEF_EXCEPTION(Exception, std::runtime_error);
00072 
00073   const unsigned int SR_COLS = 176;
00074   const unsigned int SR_ROWS = 144;
00075 
00076   class SwissRanger
00077   {
00078     public:
00079       SwissRanger ();
00080       ~SwissRanger ();
00081 
00082       int open ();
00083       int close ();
00084 
00085       void readData (std_msgs::PointCloud &cloud, std_msgs::ImageArray &images);
00086 
00087       int setAutoIllumination (bool on);
00088       int setIntegrationTime (int time);
00089       int getIntegrationTime ();
00090       int setModulationFrequency (int freq);
00091       int getModulationFrequency ();
00092       int setAmplitudeThreshold (int thresh);
00093       int getAmplitudeThreshold ();
00094 
00095       // SwissRanger specific values
00096       unsigned int rows_, cols_, inr_;
00097       std::string device_id_;
00098       std::string lib_version_;
00099     
00100       // Get/Set filtering values
00101       bool getPCDFilter () { return this->pcd_filter_; }
00102       void setPCDFilter (bool filter) { this->pcd_filter_ = filter; }
00103 
00104       bool getUndistortImage (int img_type)
00105       {
00106         switch (img_type)
00107         {
00108           case SR_IMG_DISTANCE:   return (this->undistort_distance_);
00109           case SR_IMG_AMPLITUDE:  return (this->undistort_amplitude_);
00110           case SR_IMG_CONFIDENCE: return (this->undistort_confidence_);
00111         }
00112       }
00113       void setUndistortImage (int img_type, bool filter)
00114       {
00115         switch (img_type)
00116         {
00117           case SR_IMG_DISTANCE:   { this->undistort_distance_   = filter; break; } 
00118           case SR_IMG_AMPLITUDE:  { this->undistort_amplitude_  = filter; break; }
00119           case SR_IMG_CONFIDENCE: { this->undistort_confidence_ = filter; break; }
00120         }
00121       }
00122 
00123     private:
00124       // device identifier
00125       CMesaDevice* srCam_;
00126 
00127       ImgEntry* imgEntryArray_;
00128       float *buffer_, *xp_, *yp_, *zp_;
00129 
00130       int integration_time_, modulation_freq_;
00131 
00132       std::string getDeviceString ();
00133       std::string getLibraryVersion ();
00134 
00135       // Used for rotating images with 180 deg around their centre internally
00136       void rotateImage180 (uint8_t *img, uint8_t *rot_img, int width, int height);
00137       
00138       // Stores the intrinsic camera matrix and the distortion coefficients after calibration
00139       CvMat *intrinsic_, *distortion_;
00140       void undistort (uint8_t *img, uint8_t *un_img, int width, int height);
00141       void contours (uint8_t *img, uint8_t *con_img, int width, int height, int threshold);
00142       double getAngle (float px, float py, float pz, float qx, float qy, float qz);
00143 
00144       bool pcd_filter_;       // in-driver PCD filtering 
00145       bool undistort_distance_, undistort_amplitude_, undistort_confidence_;
00146   };
00147 };
00148 
00149 #endif


composite_swissranger
Author(s): Radu Bogdan Rusu (rusu@cs.tum.edu)
autogenerated on Mon Oct 6 2014 08:11:48