$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 #include <cstdio> 00033 #include <vector> 00034 #include <opencv/cv.h> 00035 #include <opencv/highgui.h> 00036 #include <ros/node_handle.h> 00037 #include <deprecated_msgs/ImageArray.h> 00038 00039 00040 00041 extern "C" { 00046 typedef struct AVPicture { 00047 uint8_t *data[4]; 00048 int linesize[4]; 00049 } AVPicture; 00050 00051 00052 00053 // Uncomment this macro for using a video input file 00054 int img_convert(AVPicture *dst, int dst_pix_fmt, 00055 const AVPicture *src, int src_pix_fmt, 00056 int src_width, int src_height){ 00057 return(0); 00058 00059 } 00060 } 00061 00062 00063 using namespace std; 00064 using namespace deprecated_msgs; 00065 00066 class SwissRangerViewer 00067 { 00068 public: 00069 ros::NodeHandle node_; 00070 ros::Subscriber sr_sub_ ; 00071 //public: 00072 // ImageArray images_sr; 00073 char key; 00074 int imgnum; 00075 IplImage *sr_img1, *sr_img2, *sr_img3; 00076 00077 //SwissRangerViewer (ros::NodeHandle& anode) : node_ (anode) 00078 SwissRangerViewer () 00079 { 00080 cvNamedWindow ("sr4k - distance", CV_WINDOW_AUTOSIZE); 00081 cvNamedWindow ("sr4k - amplitude", CV_WINDOW_AUTOSIZE); 00082 cvNamedWindow ("sr4k - confidence", CV_WINDOW_AUTOSIZE); 00083 sr_sub_ = node_.subscribe<deprecated_msgs::ImageArray> 00084 ("/swissranger_test/images_sr", 10, &SwissRangerViewer::image_sr_cb, this); 00085 sr_img1 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1); 00086 sr_img2 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1); 00087 sr_img3 = cvCreateImage (cvSize (1, 1), IPL_DEPTH_16U, 1); 00088 imgnum = 0; 00089 } 00090 00091 void image_sr_cb(const deprecated_msgs::ImageArray::ConstPtr &images_sr) 00092 { 00093 cvReleaseImage (&sr_img1); 00094 cvReleaseImage (&sr_img2); 00095 cvReleaseImage (&sr_img3); 00096 00097 fprintf (stderr, "Received %d images of size:", images_sr->get_images_size ()); 00098 for (unsigned int j = 0; j < images_sr->get_images_size (); j++) 00099 { 00100 fprintf (stderr, " %dx%d and type %s,", images_sr->images[j].width, images_sr->images[j].height, images_sr->images[j].colorspace.c_str ()); 00101 if (images_sr->images[j].width == 0 || images_sr->images[j].height == 0) 00102 return; 00103 } 00104 fprintf (stderr, "\n"); 00105 00106 sr_img1 = cvCreateImage (cvSize (images_sr->images[0].width, images_sr->images[0].height), IPL_DEPTH_16U, 1); 00107 sr_img2 = cvCreateImage (cvSize (images_sr->images[1].width, images_sr->images[1].height), IPL_DEPTH_16U, 1); 00109 sr_img3 = cvCreateImage (cvSize (images_sr->images[2].width, images_sr->images[2].height), IPL_DEPTH_16U, 1); 00110 00111 memcpy (sr_img1->imageData, &(images_sr->images[0].data[0]), sr_img1->imageSize); 00112 memcpy (sr_img2->imageData, &(images_sr->images[1].data[0]), sr_img2->imageSize); 00113 memcpy (sr_img3->imageData, &(images_sr->images[2].data[0]), sr_img3->imageSize); 00114 00115 cvShowImage ("sr4k - distance", sr_img1); 00116 00117 IplImage* sr_img2_rot = cvCloneImage (sr_img2); 00118 sr_img2 = cvCreateImage (cvGetSize (sr_img2_rot), IPL_DEPTH_8U, 3); 00119 if (sr_img2_rot->depth == IPL_DEPTH_16U) 00120 cvConvertImage (sr_img2_rot, sr_img2, CV_CVTIMG_SWAP_RB); 00121 00122 IplImage *sr_img2_a = cvCreateImage (cvGetSize (sr_img2_rot), IPL_DEPTH_8U, 1); 00123 cvCvtColor (sr_img2, sr_img2_a, CV_RGB2GRAY); 00124 00125 IplImage *sr_img2_rot_hist = cvCloneImage (sr_img2_a); 00126 cvEqualizeHist (sr_img2_a, sr_img2_rot_hist); 00127 00128 cvReleaseImage (&sr_img2); 00129 00130 sr_img2 = cvCreateImage (cvSize (sr_img2_rot_hist->width << 1, sr_img2_rot_hist->height << 1), IPL_DEPTH_8U, 1); 00131 cvResize (sr_img2_rot_hist, sr_img2, CV_INTER_NN); 00132 cvShowImage ("sr4k - amplitude", sr_img2); 00133 00134 cvReleaseImage (&sr_img2_rot_hist); 00135 cvReleaseImage (&sr_img2_rot); 00136 00137 cvShowImage ("sr4k - confidence", sr_img3); 00138 cvWaitKey (20); 00139 } 00140 00141 }; 00142 00143 int 00144 main (int argc, char **argv) 00145 { 00146 ros::init (argc, argv, "swissranger_viewer"); 00147 //ros::NodeHandle nh("~"); 00148 SwissRangerViewer n; 00149 ros::spin (); 00150 00151 return (0); 00152 }