project_light.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 // Modified by Gary Bradski 2/26/09 to allow for variable random pattern
00035 
00036 #include <signal.h>
00037 #include <stdlib.h>
00038 #include <math.h>
00039 #include <ctype.h>
00040 #include <time.h>
00041 
00042 #include "opencv/cv.h"
00043 #include "opencv/cvaux.h"
00044 #include "opencv/highgui.h"
00045 #include "opencv/cxcore.h"
00046 
00047 #include "ros/node_handle.h"
00048 
00049 #include "std_msgs/UInt8.h"
00050 
00051 #include "limits.h"
00052 
00053 #include <iostream>
00054 
00055 #define WD 1280
00056 #define HT 1024
00057 #define BLOCK_SIZE 2
00058 #define MAX_BLOCK_SIZE 128
00059 #define MIN_BLOCK_SIZE 1
00060 
00061 //#define COLOR
00062 inline int min(int a, int b){
00063   return (a<b)?a:b;
00064 }
00065 
00066 
00067 bool BlankScreen = true;
00068 bool BlankScreenPrevious = false;
00069 IplImage* Rbp;
00070 IplImage* BlankImage;
00071 IplImage *current;
00072 
00073 std_msgs::UInt8 status_out;
00074 
00075 using namespace std;
00076 
00077 class LightProjector : public ros::NodeHandle
00078 {
00079 public:
00080   int alt2_on1_off0;                            //Turn screen flashing to alternate (2), always on (1) or off (0)
00081   int _offset, _range, _block_size;     //Add offset to random pattern, pattern range, square size of pattern
00082   bool _ramp;                                                   //If true, add a ramp of decreasing brightness from top to bottom
00083   float ffactor;                                                //Speed up factor for alternation frequency
00084   
00085   LightProjector(ros::NodeHandle &n) : alt2_on1_off0(1), _offset(0), _range(128), _block_size(BLOCK_SIZE), _ramp(false), ffactor(1.0),  n_(n)
00086   {
00087     n_.param("frequency", frequency, 1.0);
00088     n_.setParam("pattern_on", 1);
00089     cout << "frequency = " << frequency << endl;
00090     status_publisher = n_.advertise<std_msgs::UInt8>("projector_status", 10);
00091   }
00092   // MAKE RANDOM PATTERN IMAGE:
00093   // rbp                -- random block pattern image
00094   // offset             -- Add this possibly negative value to the number, clip [0,255] (for random number generation)
00095   // range              -- Vary numbers this much, clip [0,255] (for random number generation)
00096   // blocks_size        -- How big the random squares should be
00097   // ramp               -- if on, linear ramp from top (bright) to bottom (dimmer)
00098   void grayscale_rbp(IplImage *rbp, int offset = 0, int range = 128, int block_size = BLOCK_SIZE, bool ramp = false){
00099     printf("Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",offset,range,block_size,ramp);
00100     int w = rbp->width;
00101     int h = rbp->height;
00102     int ws = rbp->widthStep;
00103     unsigned char *drbp = ((unsigned char *)(rbp->imageData));
00104     
00105     int i,j,x,y, rn;
00106     int bs = block_size;
00107     float ramp_down_step = 0.5*(float)range/(float)h;
00108     int ramp_down = 0;
00109     
00110     //Step accross image
00111     for(y=0; y<h; y+=bs){
00112       for(x=0; x<w; x+=bs){
00113         //      rn = (rand()%2)*128;// 256;
00114         rn = (rand()%range)+offset;
00115         if(ramp) {
00116           ramp_down = (int)(ramp_down_step*(float)y);
00117           rn -= ramp_down;
00118         }
00119         if(rn < 0) rn = 0;
00120         if(rn > 255) rn = 255;
00121         
00122         //Do a random block
00123         for(j=0; j<bs; j++){
00124           for(i=0; i<bs; i++){
00125             drbp[(y+j)*ws+x+i] = rn;
00126           }
00127         } 
00128       }
00129     }
00130   }
00131   
00132   bool spin()
00133   {
00134     int key = 'a';
00135     while (ok())
00136       {
00137         getParam("pattern_on", alt2_on1_off0);
00138         key = cvWaitKey(10)&0xFF;       
00139         if(key == 27 ) //ESC
00140           break;
00141         switch(key)
00142           {
00143           case 'h':
00144             cout << "\nNode to flip projector pattern on and off\n" << endl;
00145             cout << "  a,1,0  a,2:Alternate projector pattern, 1:On, 0:Off" << endl;
00146             cout << "  o,O    offset down, up" << endl;
00147             cout << "  r,R    range down, up" << endl;
00148             cout << "  b,B    block_size down, up" << endl;
00149             cout << "  d      dim bottom of image toggle" << endl;
00150             cout << "--------\n" << endl;
00151             break;
00152           case '0': //Keep projector off
00153             alt2_on1_off0 = 0;
00154             ffactor = 40.0;
00155             break;
00156           case '1': //Keep projector on
00157             alt2_on1_off0 = 1;
00158             ffactor = 40.0;
00159             break;
00160           case 'a': //Alternate the projector
00161           case '2':
00162             alt2_on1_off0 = 2;
00163             ffactor = 1.0;
00164             break;
00165             //void grayscale_rbp(IplImage *rbp, int offset = 0, int range = 128, int block_size = BLOCK_SIZE, bool ramp = false)
00166           case 'o':
00167             _offset -= 10;
00168             if(_offset < -255) _offset = -255;
00169             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00170             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00171             break;
00172           case 'O':
00173             _offset += 10;
00174             if(_offset > 255) _offset = 255;
00175             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00176             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00177             break;
00178           case 'r':
00179             _range -= 10;
00180             if(_range < 10) _range = 10;
00181             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00182             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00183             break;
00184           case 'R':
00185             _range += 10;
00186             if(_range > 255) _range = 400;
00187             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00188             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00189             break;
00190           case 'b':
00191             _block_size /= 2;
00192             if(_block_size <= 0) _block_size = MIN_BLOCK_SIZE;
00193             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00194             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00195             break;
00196           case 'B':
00197             _block_size *= 2;
00198             if(_block_size > 80) _block_size = MAX_BLOCK_SIZE;
00199             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00200             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00201             break;
00202           case 'd':
00203             _ramp = !_ramp;
00204             printf("New Pattern: offset(%d), range(%d), block_size(%d), ramp(%d)\n",_offset,_range,_block_size,_ramp);
00205             grayscale_rbp(Rbp, _offset, _range, _block_size, _ramp);
00206             break;
00207           case 's':
00208             cvSaveImage("pattern.png", Rbp);
00209             ROS_INFO("Saved image pattern.png");
00210             break;
00211           }
00212         
00213         //IF PRODUCING ALTERNATING RANDOM IMAGE OR BLACK SCREEN
00214         if(alt2_on1_off0 == 2)
00215           {  
00216             if(!BlankScreen){
00217               cvShowImage("RBP Projector", Rbp); //This won't show until function exit
00218             }
00219             else{
00220               cvShowImage("RBP Projector", BlankImage); //This won't show until function exit
00221             }
00222           }
00223         //IF ALWAYS ON
00224         else if(alt2_on1_off0 == 1) {
00225           cvShowImage("RBP Projector", Rbp);
00226           BlankScreen = 1;
00227         }
00228         //ELSE ALWAYS OFF
00229         else { //alt2_on1_off0 == 0
00230           cvShowImage("RBP Projector", BlankImage);
00231           BlankScreen = 0;
00232         }
00233         
00234         usleep(1000000.0/(frequency*ffactor));        //FREQUENCY CONVERTS TO SECONDS, FFACTOR SCALES THAT. DURATION OF FLASH
00235         if(alt2_on1_off0 == 2) BlankScreen = !BlankScreen;
00236         status_out.data = (int)BlankScreen;
00237         
00238         status_publisher.publish(status_out);
00239         //        printf("Speckle(1) Blank(0) = %d \n",(int)(BlankScreen));       
00240       }
00241     return true;
00242   }
00243   
00244 private:
00245   double frequency;
00246   ros::Publisher status_publisher;
00247   ros::NodeHandle n_;
00248 };
00249 
00250 int main(int argc, char **argv){
00251   for(int i = 0; i<argc; ++i)
00252     cout << "(" << i << "): " << argv[i] << endl;
00253   ros::init(argc, argv, "light_projector");
00254   ros::NodeHandle n("~");
00255   LightProjector  LP(n);
00256 
00257   Rbp = cvCreateImage(cvSize(WD,HT), 8, 1);
00258   BlankImage = cvCreateImage(cvSize(WD,HT), 8, 1);
00259   for(int j=0; j<BlankImage->height; j++)
00260     for(int i=0; i<BlankImage->width; i++)
00261       ((unsigned char *)(BlankImage->imageData))[j*BlankImage->widthStep+i] = 0;
00262   
00263   cvNamedWindow("RBP Projector", 0);
00264   cvResizeWindow("RBP Projector", WD, HT);
00265   cvMoveWindow("RBP Projector", 0, 0);
00266   
00267   cvShowImage("RBP Projector", BlankImage);
00268   
00269   LP.grayscale_rbp(Rbp); //Set initial image
00270   
00271   srand ( time(NULL) );
00272   
00273   LP.spin();
00274   
00275   cvReleaseImage(&Rbp);
00276   cvReleaseImage(&BlankImage);
00277   cvDestroyWindow("RBP Projector");
00278   
00279   return 0;
00280   
00281 }
00282 
00283 


projected_light
Author(s): Dejan Pangercic, Andreas Leha
autogenerated on Mon Oct 6 2014 09:24:44