$search
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