sentis_tof_m100_node.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * Copyright (c) 2013
00003  * VoXel Interaction Design GmbH
00004  *
00005  * @author Angel Merino Sastre
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy
00008  * of this software and associated documentation files (the "Software"), to deal
00009  * in the Software without restriction, including without limitation the rights
00010  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00011  * copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in
00015  * all copies or substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00018  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00019  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00020  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00021  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00022  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00023  * THE SOFTWARE.
00024  *
00025  ******************************************************************************/
00026 
00042 #include <m100api.h>
00043 
00044 #include <ros/console.h>
00045 #include <ros/ros.h>
00046 #include <tf/transform_listener.h>
00047 #include <ros/publisher.h>
00048 #include <pcl_ros/point_cloud.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl/io/pcd_io.h>
00051 #include <pcl/filters/statistical_outlier_removal.h>
00052 
00053 #include <stdio.h>
00054 #include <time.h>
00055 #include <sstream>
00056 
00057 #include <sentis_tof_m100/sentis_tof_m100Config.h>
00058 #include <dynamic_reconfigure/server.h>
00059 
00060 /* Registers for Filter Configuration */
00061 #define ImgProcConfig                           0x01E0
00062 #define FilterMedianConfig                      0x01E1 
00063 #define FilterAverageConfig                     0x01E2 
00064 #define FilterGaussConfig                       0x01E3
00065 #define FilterSLAFconfig                        0x01E5
00066 
00067 /*Bitmasks for ImgProcConfig*/
00068 #define Median_Filter                   1       // Bit[0]
00069 #define Average_Filter                  2       // Bit[1]
00070 #define Gauss_Filter                    4       // Bit[2]
00071 #define Sliding_Average                 16      // Bit[4]
00072 #define Wiggling_Compensation           64      // Bit[6]
00073 #define FPPN_Compensation               128     // Bit[7]
00074 #define ModFreq_Scaling                 256     // Bit[8]
00075 #define Scaling_MM                      512     // Bit[9]
00076 #define Additive_Offset                 1024    // Bit[10]
00077 #define Temperature_Compensation        2048    // Bit[11]
00078 #define Scaling_DistCalibGradient       4096    // Bit[12]
00079 #define Scaling_DistCalibOffset         8192    // Bit[13]
00080 
00081 typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
00082 
00090 int integrationTime;
00091 int modulationFrequency;
00092 int frameRate;
00093 
00094 bool amplitudeFilterOn;
00095 float amplitudeThreshold;
00096 
00097 unsigned short filterConfig, averageConfig, gaussConfig;
00098 
00099 bool medianFilter, averageFilter, gaussFilter, slidingAverage, wigglingCompensation, FPPNCompensation, modFreqScaling, scalingMM,
00100         additiveOffset, temperatureCompensation, scalingDistCalibGradient, scalingDistCalibOffset;
00101 
00102 bool filterAverageConfigPixels, filterGaussConfigPixels;
00103 int medianConfigIters, averageConfigIters, gaussConfigIters, SLAConfigWindows;
00104 
00105 bool first;
00109 T_SENTIS_HANDLE handle;
00110 T_ERROR_CODE error;
00111 T_SENTIS_DATA_HEADER header;
00112 
00116 ros::Publisher pub_non_filtered;
00117 ros::Publisher pub_filtered;
00118 
00119 int help() {
00120                 std::cout << "\n Using help for sentis_tof_m100_ros_pkg\n"
00121                 " You can set the configuration values for the camera. If any option is missing the server parameter value or in last case, the default value will be used: \n" << std::endl;
00122         std::cout << " Usage:\n rosrun sentis_tof_m100 sentis_tof_m100_node <options> "<< std::endl
00123                 << "-tcp_ip *TCP IP Addresss* \n\tIp address for the control connection \n\t(string, i.e: 192.168.0.10) "<< std::endl
00124                 << "-tcp_port *Port for tcp* \n\tDefines the port used for the control connection \n\t(unsigned short, i.e: 10001) "<< std::endl
00125                 << "-udp_ip *UDP IP Addresss* \n\tMulticast ip address for the data connection \n\t(string, i.e: 224.0.0.1) "<< std::endl
00126                 << "-udp_port *Port for udp* \n\tDefines the port used for the data connection \n\t(unsigned short, i.e: 10001) "<< std::endl
00127                 << "-it *Integration_Time* \n\tIntegration time(in usec) for the sensor \n\t(min: 50 | max: 7000 | default: 1500) "<< std::endl
00128                 << "-mf  *Modulation_Frequency* \n\tSet the modulation frequency(Hz) of the sensor \n\t(min: 5000 | max: 30000 | default: 20000) "<< std::endl
00129                 << "-fr *Frame_Rate* \n\tSet the frame rate of the camera by setting the Phase Time \n\t(min: 1 | max: 45 | default: 40)" << std::endl
00130 
00131                 << "-mef *MedianFilter* \n\tSet on or off the Median Filter. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00132                 << "-avf *AverageFilter* \n\tSet on or off the Average Filter. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00133                 << "-gaf *GaussFilter* \n\tSet on or off the Gauss Filter. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00134                 << "-sla *SlidingAverage* \n\tSet on or off the Sliding Average. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00135                 << "-wic *WigglingCompensation* \n\tSet on or off the Wiggling Compensation. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00136                 << "-fppnc *FPPNCompensation* \n\tSet on or off the FPPN Compensation. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00137                 << "-mfs *ModFreqScaling* \n\tSet on or off the ModFreq Scaling. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00138                 << "-smm *Scalingmm* \n\tSet on or off the Scaling to [mm]. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00139                 << "-aos *AdditiveOffset* \n\tSet on or off the Additive Offset. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00140                 << "-tmc *TemperatureCompensation* \n\tSet on or off the Temperature Compensation. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00141                 << "-sdcg *ScalingDistCalibGradient* \n\tSet on or off the Scaling via register DistCalibGradient. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00142                 << "-sdco *ScalingDistCalibOffset* \n\tSet on or off the Scaling via register DistCalibOffset. \n\t(OFF: 0 | ON: any other integer value |  ON if not set ) " << std::endl 
00143 
00144                 << "-mefite *FilterMedian_Config* \n\tSet the nº of iteration for the Media filter. \n\t(min: 1 | max: 255 | default: 1) "<< std::endl
00145                 << "-avfpix *FilterAverage_Config_Pixels* \n\tSet pixel matrix for the Average filter. \n\t(3x3: 0 | 5x5: 1 | Default: 3x3 ) " << std::endl
00146                 << "-avfite *FilterAverage_Config_Iters* \n\tSet the nº of iteration for the Average filter. \n\t(min: 1 | max: 255 | default: 1) "<< std::endl
00147                 << "-gafpix *FilterGauss_Config_Pixels* \n\tSet pixel matrix for the Gauss filter. \n\t(3x3: 0 | 5x5: 1 | Default: 3x3 ) " << std::endl
00148                 << "-gafite *FilterGauss_Config_Iters* \n\tSet the nº of iteration for the Gauss filter. \n\t(min: 1 | max: 255 | default: 1) "<< std::endl
00149                 << "-slacw *FilterSLAF_config* \n\tSet the SLAF filter windows size. \n\t(min: 1 | max: 255 | default: 1) "<< std::endl
00150 
00151                 << "-af *Amplitude_Filter_On* \n\tWhether to apply amplitude filter or not. Image pixels with amplitude values less than the threshold will be filtered out \n\t(ON: if set | OFF: default) " << std::endl
00152                 << "-at *Amplitude_Threshold* \n\tWhat should be the amplitude filter threshold. Image pixels with smaller aplitude values will be filtered out. Amplitude Filter Status should be true to use this filter \n\t(min: 0 | max: 2500 | default: 0) "<< std::endl
00153                 << "\n Example:" << std::endl
00154                 << "rosrun sentis_tof_m100 sentis_tof_m100 -tcp_ip 192.168.0.10 -tcp_port 10001 -it 1500 -mf 20000 -fr 20 \n" << std::endl;
00155         exit(0);
00156 } //print_help
00157 
00158 void callback(sentis_tof_m100::sentis_tof_m100Config &config, uint32_t level) {
00159         // Check the configuration parameters with those given in the initialization
00160         if(first) {
00161                 config.Integration_Time = integrationTime;
00162                 config.Modulation_Frequency = modulationFrequency;
00163                 config.Frame_Rate = frameRate;
00164                 
00165                 config.Amplitude_Filter_On = amplitudeFilterOn;
00166                 config.Amplitude_Threshold = amplitudeThreshold;
00167                 
00168                 frameRate = integrationTime = modulationFrequency = -1;
00169                                 
00170                 config.MedianFilter = medianFilter;
00171                 config.AverageFilter = averageFilter;
00172                 config.GaussFilter = gaussFilter;
00173                 config.SlidingAverage = slidingAverage;
00174                 config.WigglingCompensation = wigglingCompensation; 
00175                 config.FPPNCompensation = FPPNCompensation;
00176                 config.ModFreqScaling = modFreqScaling;
00177                 config.Scalingmm = scalingMM;
00178                 config.AdditiveOffset = additiveOffset;
00179                 config.TemperatureCompensation = temperatureCompensation;
00180                 config.ScalingDistCalibGradient = scalingDistCalibGradient;
00181                 config.ScalingDistCalibOffset = scalingDistCalibOffset;
00182                 
00183                 config.FilterMedian_Config = medianConfigIters;
00184                 config.FilterAverage_Config_Pixels = filterAverageConfigPixels;
00185                 config.FilterAverage_Config_Iters = averageConfigIters;
00186                 config.FilterGauss_Config_Pixels = filterGaussConfigPixels;
00187                 config.FilterGauss_Config_Iters = gaussConfigIters;
00188                 config.FilterSLAF_config = SLAConfigWindows;
00189                 medianConfigIters = averageConfigIters = gaussConfigIters = SLAConfigWindows = filterConfig = -1;
00190         }
00191 
00192         if(integrationTime != config.Integration_Time) {
00193                 integrationTime = config.Integration_Time;
00194                 error = STSwriteRegister(handle, IntegrationTime, integrationTime );
00195                 if(error != 0) {
00196                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00197                     STSclose(handle);
00198                     exit (1);
00199                 }
00200         }
00201 
00202         if(modulationFrequency != config.Modulation_Frequency) {
00203                 modulationFrequency = config.Modulation_Frequency;
00204                 error = STSwriteRegister(handle, ModulationFrecuency, modulationFrequency );
00205                 
00206                 if(error != 0) {
00207                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00208                     STSclose(handle);
00209                     exit (1);
00210                 }
00211         }
00212 
00213         if(frameRate != config.Frame_Rate) {
00214                 frameRate = config.Frame_Rate;
00215                 error = STSwriteRegister(handle, FrameRate, frameRate );
00216                 if(error != 0) {
00217                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00218                     STSclose(handle);
00219                     exit (1);
00220                 }
00221         }
00222         
00223         unsigned short filterConfNew = 0;
00224         if (config.MedianFilter)
00225                 filterConfNew |= Median_Filter;
00226         if (config.AverageFilter)
00227                 filterConfNew |= Average_Filter; 
00228         if (config.GaussFilter)
00229                 filterConfNew |= Gauss_Filter; 
00230         if (config.SlidingAverage)
00231                 filterConfNew |= Sliding_Average; 
00232         if (config.WigglingCompensation)
00233                 filterConfNew |= Wiggling_Compensation;  
00234         if (config.FPPNCompensation)
00235                 filterConfNew |= FPPN_Compensation; 
00236         if (config.ModFreqScaling)
00237                 filterConfNew |= ModFreq_Scaling; 
00238         if (config.Scalingmm)
00239                 filterConfNew |= Scaling_MM; 
00240         if (config.AdditiveOffset)
00241                 filterConfNew |= Additive_Offset; 
00242         if (config.TemperatureCompensation)
00243                 filterConfNew |= Temperature_Compensation;
00244         if (config.ScalingDistCalibGradient)
00245                 filterConfNew |= Scaling_DistCalibGradient;
00246         if (config.ScalingDistCalibOffset)
00247                 filterConfNew |= Scaling_DistCalibOffset;
00248 
00249         // TODO remove DEBUG
00250         //ROS_INFO_STREAM(filterConfNew);       
00251         //unsigned short res;
00252         //STSreadRegister(handle, ImgProcConfig, &res, 0, 0);
00253         //printf("ImgProcConfig : %#x   %d\n", res, res);       
00254 
00255         if (filterConfNew != filterConfig) {
00256                 filterConfig = filterConfNew;
00257                 error = STSwriteRegister(handle, ImgProcConfig, filterConfig );
00258                 if(error != 0) {
00259                     // TODO remove DEBUG
00260                     //ROS_WARN_STREAM("errno: " << errno);
00261 
00262                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00263                     STSclose(handle);
00264                     exit (1);
00265                 }
00266         }
00267         
00268 
00269         if (medianConfigIters != config.FilterMedian_Config) {
00270                 medianConfigIters = config.FilterMedian_Config;
00271                 error = STSwriteRegister(handle, FilterMedianConfig, (unsigned short)medianConfigIters );
00272                 if(error != 0) {
00273                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00274                     STSclose(handle);
00275                     exit (1);
00276                 }
00277         }
00278         
00279         filterConfNew = config.FilterAverage_Config_Iters << 8;
00280         if (config.FilterAverage_Config_Pixels)
00281                 filterConfNew++;
00282         if (filterConfNew != averageConfig) {
00283                 averageConfig = filterConfNew;
00284                 error = STSwriteRegister(handle, FilterAverageConfig, averageConfig );
00285                 if(error != 0) {
00286                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00287                     STSclose(handle);
00288                     exit (1);
00289                 }
00290         }
00291         
00292         filterConfNew = config.FilterGauss_Config_Iters << 8;
00293         if (config.FilterGauss_Config_Pixels)
00294                 filterConfNew++;
00295         if (filterConfNew != gaussConfig) {
00296                 gaussConfig = filterConfNew;
00297                 error = STSwriteRegister(handle, FilterGaussConfig, gaussConfig);
00298                 if(error != 0) {
00299                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00300                     STSclose(handle);
00301                     exit (1);
00302                 }
00303         }
00304         
00305         if (SLAConfigWindows != config.FilterSLAF_config) {
00306                 SLAConfigWindows = config.FilterSLAF_config;
00307                 error = STSwriteRegister(handle, FilterSLAFconfig, SLAConfigWindows);
00308                 if(error != 0) {
00309                     ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00310                     STSclose(handle);
00311                     exit (1);
00312                 }
00313         }
00314 
00315         amplitudeFilterOn = config.Amplitude_Filter_On;
00316         amplitudeThreshold = config.Amplitude_Threshold;
00317         
00318         // TODO remove DEBUG
00319         //STSreadRegister(handle, ImgProcConfig, &res, 0, 0);
00320         //printf("ImgProcConfig : %#x   %d\n", res, res);
00321         
00322 }
00323 
00327 int initialize(int argc, char *argv[],ros::NodeHandle nh){
00328         /*
00329          * Inital Setup for parameters
00330          */
00331         integrationTime = 1500;
00332         modulationFrequency = 20000;
00333         frameRate = 40;
00334         
00335         /*Default ImgProcConfig values: "2FC1"*/
00336         medianFilter = true;
00337         averageFilter = gaussFilter = slidingAverage = false;
00338         wigglingCompensation = FPPNCompensation = modFreqScaling = scalingMM = additiveOffset = temperatureCompensation =  true; 
00339         scalingDistCalibGradient = false;
00340         scalingDistCalibOffset = true;
00341         
00342         medianConfigIters = averageConfigIters = gaussConfigIters = 1;
00343          
00344         filterAverageConfigPixels = filterGaussConfigPixels = false;
00345         
00346         filterConfig = 0x2FC1;
00347         averageConfig = gaussConfig = 0x0100;
00348         SLAConfigWindows = 5;
00349         
00350         amplitudeFilterOn = false;
00351         amplitudeThreshold = 0;
00352         
00353         /*
00354          * Camera Initialization
00355          */
00356         T_SENTIS_CONFIG sentisConfig;
00357         // Default configuration
00358         sentisConfig.tcp_ip = "192.168.0.10";
00359         sentisConfig.udp_ip = "224.0.0.1";
00360         sentisConfig.udp_port = 10002;
00361         sentisConfig.tcp_port = 10001;
00362         sentisConfig.flags = HOLD_CONTROL_ALIVE;
00363         
00364         std::string nodeName = ros::this_node::getName();
00365         
00366         // Overiding default configuration with parameter server.
00367         std::string aux_tcpIp,aux_udpIp;
00368         if (nh.getParam(nodeName+"/network/tcp_ip",aux_tcpIp))
00369                 sentisConfig.tcp_ip = aux_tcpIp.c_str();
00370         if (nh.getParam(nodeName+"/network/udp_ip",aux_udpIp))
00371                 sentisConfig.udp_ip = aux_udpIp.c_str();
00372         
00373         int aux_port;
00374         if (nh.getParam(nodeName+"/network/tcp_port",aux_port))
00375                 sentisConfig.tcp_port = (unsigned short)aux_port;
00376         if (nh.getParam(nodeName+"/network/udp_port",aux_port))
00377                 sentisConfig.udp_port = (unsigned short)aux_port;  
00378         
00379         nh.getParam(nodeName+"/integrationTime",integrationTime);
00380         nh.getParam(nodeName+"/modulationFrecuency",modulationFrequency);
00381         nh.getParam(nodeName+"/frameRate",frameRate);
00382 
00383         nh.getParam(nodeName+"/amplitudeFilterOn",amplitudeFilterOn);
00384         double aux_double;
00385         if (nh.getParam(nodeName+"/amplitudeThreshold",aux_double))
00386                 amplitudeThreshold = (float)aux_double;
00387         
00388         // Filters flags        
00389         nh.getParam(nodeName+"/filters/mediaFilter",medianFilter);
00390         nh.getParam(nodeName+"/filters/averageFilter",averageFilter);
00391         nh.getParam(nodeName+"/filters/gaussFilter",gaussFilter);
00392         nh.getParam(nodeName+"/filters/slidingAverage",slidingAverage);
00393         nh.getParam(nodeName+"/filters/wigglingCompensation",wigglingCompensation);
00394         nh.getParam(nodeName+"/filters/FPPNCompensation",FPPNCompensation);
00395         nh.getParam(nodeName+"/filters/modFreqScaling",modFreqScaling);
00396         nh.getParam(nodeName+"/filters/scalingMM",scalingMM);
00397         nh.getParam(nodeName+"/filters/additiveOffset",additiveOffset);
00398         nh.getParam(nodeName+"/filters/temperatureCompensation",temperatureCompensation);
00399         nh.getParam(nodeName+"/filters/scalingDistCalibGradient",scalingDistCalibGradient);
00400         nh.getParam(nodeName+"/filters/scalingDistCalibOffset",scalingDistCalibOffset);
00401         
00402         // Filter configurations
00403         nh.getParam(nodeName+"/filters/FilterMedianConfig",medianConfigIters);
00404         nh.getParam(nodeName+"/filters/averageConfig_Iters",averageConfigIters);
00405         nh.getParam(nodeName+"/filters/filterAverageConfig_Pixels",filterAverageConfigPixels);
00406         nh.getParam(nodeName+"/filters/gaussConfig_Iters",gaussConfigIters);
00407         nh.getParam(nodeName+"/filters/filterGaussConfig_Pixels",filterGaussConfigPixels);
00408         nh.getParam(nodeName+"/filters/SLAConfigWindows",SLAConfigWindows);
00409         
00410         char aux_char[20]; 
00411         for( int i = 1; i < argc; i++) {
00412                 if( std::string(argv[i]) == "-tcp_ip" ) {
00413                         aux_tcpIp = argv[++i];
00414                         sentisConfig.tcp_ip = aux_tcpIp.c_str();
00415                 }
00416                 else if( std::string(argv[i]) == "-udp_ip" ) {
00417                         aux_udpIp = argv[++i];
00418                         sentisConfig.udp_ip = aux_udpIp.c_str();
00419                 }
00420                 else if( std::string(argv[i]) == "-tcp_port" ) {
00421                         if( sscanf(argv[++i], "%d", &aux_port) != 1 
00422                                 || aux_port < 0 || aux_port > 65535 ) {
00423                                 ROS_WARN("*invalid tcp port");
00424                                 return help();
00425                         }
00426                         sentisConfig.tcp_port = (unsigned short)aux_port;
00427                 }
00428                 else if( std::string(argv[i]) == "-udp_port" ) {
00429                         if( sscanf(argv[++i], "%d", &aux_port) != 1 
00430                                 || aux_port < 0 || aux_port > 65535 ) {
00431                                 ROS_WARN("*invalid udp port");
00432                                 return help();
00433                         }
00434                         sentisConfig.udp_port = (unsigned short)aux_port;
00435                 }
00436                 else if( std::string(argv[i]) == "-it" ) {
00437                         if( sscanf(argv[++i], "%d", &integrationTime) != 1 
00438                                 || integrationTime < 50 || integrationTime > 7000 ) {
00439                                 ROS_WARN("*invalid integration time");
00440                                 return help();
00441                         }
00442                 }
00443                 else if( std::string(argv[i]) == "-mf" ) {
00444                         if( sscanf(argv[++i], "%d", &modulationFrequency) != 1 
00445                                 || modulationFrequency < 5000 || integrationTime > 30000 ) {
00446                                 ROS_WARN("*invalid modulation frequency");
00447                                 return help();
00448                         }
00449                 }
00450                 else if( std::string(argv[i]) == "-fr" ) {
00451                         if( sscanf(argv[++i], "%d", &frameRate) != 1 
00452                                 || frameRate < 1 || frameRate > 45 ) {
00453                                 ROS_WARN("*invalid frame rate");
00454                                 return help();
00455                         }
00456                 }
00457                 else if( std::string(argv[i]) == "-mef" ) {
00458                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00459                                 if (aux_port == 0)
00460                                         medianFilter = false;
00461                                 else
00462                                         medianFilter = true;
00463                                 continue;                       
00464                         }
00465                         ROS_WARN("*invalid value for Median Filter");
00466                         return help();
00467                 }
00468                 else if( std::string(argv[i]) == "-avf" ) {
00469                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00470                                 if (aux_port == 0)
00471                                         averageFilter = false;
00472                                 else
00473                                         averageFilter = true;
00474                                 continue;                       
00475                         }
00476                         ROS_WARN("*invalid value for Average Filter");
00477                         return help();
00478                 }
00479                 else if( std::string(argv[i]) == "-gaf" ) {
00480                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00481                                 if (aux_port == 0)
00482                                         gaussFilter = false;
00483                                 else
00484                                         gaussFilter = true;
00485                                 continue;                       
00486                         }
00487                         ROS_WARN("*invalid value for Gauss Filter");
00488                         return help();
00489                 }
00490                 else if( std::string(argv[i]) == "-sla" ) {
00491                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00492                                 if (aux_port == 0)
00493                                         slidingAverage = false;
00494                                 else
00495                                         slidingAverage = true;
00496                                 continue;                       
00497                         }
00498                         ROS_WARN("*invalid value for Sliding Average Filter");
00499                         return help();
00500                 }
00501                 else if( std::string(argv[i]) == "-wic" ) {
00502                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00503                                 if (aux_port == 0)
00504                                         wigglingCompensation = false;
00505                                 else
00506                                         wigglingCompensation = true;
00507                                 continue;                       
00508                         }
00509                         ROS_WARN("*invalid value for Wiggling Compensation");
00510                         return help();
00511                 }
00512                 else if( std::string(argv[i]) == "-fppnc" ) {
00513                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00514                                 if (aux_port == 0)
00515                                         FPPNCompensation = false;
00516                                 else
00517                                         FPPNCompensation = true;
00518                                 continue;                       
00519                         }
00520                         ROS_WARN("*invalid value for FPPN Compensation");
00521                         return help();
00522                 }
00523                 else if( std::string(argv[i]) == "-mfs" ) {
00524                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00525                                 if (aux_port == 0)
00526                                         modFreqScaling = false;
00527                                 else
00528                                         modFreqScaling = true;
00529                                 continue;                       
00530                         }
00531                         ROS_WARN("*invalid value for ModFreq Scaling");
00532                         return help();
00533                 }
00534                 else if( std::string(argv[i]) == "-smm" ) {
00535                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00536                                 if (aux_port == 0)
00537                                         scalingMM = false;
00538                                 else
00539                                         scalingMM = true;
00540                                 continue;                       
00541                         }
00542                         ROS_WARN("*invalid value for Scaling to [mm]");
00543                         return help();
00544                 }
00545                 else if( std::string(argv[i]) == "-aos" ) {
00546                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00547                                 if (aux_port == 0)
00548                                         additiveOffset = false;
00549                                 else
00550                                         additiveOffset = true;
00551                                 continue;                       
00552                         }
00553                         ROS_WARN("*invalid value for Additive Offset");
00554                         return help();
00555                 }
00556                 else if( std::string(argv[i]) == "-tmc" ) {
00557                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00558                                 if (aux_port == 0)
00559                                         temperatureCompensation = false;
00560                                 else
00561                                         temperatureCompensation = true;
00562                                 continue;                       
00563                         }
00564                         ROS_WARN("*invalid value for Temperature Compensation");
00565                         return help();
00566                 }
00567                 else if( std::string(argv[i]) == "-sdcg" ) {
00568                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00569                                 if (aux_port == 0)
00570                                         scalingDistCalibGradient = false;
00571                                 else
00572                                         scalingDistCalibGradient = true;
00573                                 continue;                       
00574                         }
00575                         ROS_WARN("*invalid value for Scaling via register DistCalibGradient");
00576                         return help();
00577                 }
00578                 else if( std::string(argv[i]) == "-sdco" ) {
00579                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00580                                 if (aux_port == 0)
00581                                         scalingDistCalibOffset = false;
00582                                 else
00583                                         scalingDistCalibOffset = true;
00584                                 continue;                       
00585                         }
00586                         ROS_WARN("*invalid value for Scaling via register DistCalibOffset");
00587                         return help();
00588                 }
00589 
00590                 else if( std::string(argv[i]) == "-mefite" ) {
00591                         if( sscanf(argv[++i], "%d", &medianConfigIters) != 1 
00592                                 || medianConfigIters < 1 || medianConfigIters > 255 ) {
00593                                 ROS_WARN("*invalid value for Median filter Iterations value");
00594                                 return help();
00595                         }
00596                 }
00597                 else if( std::string(argv[i]) == "-avfpix" ) {
00598                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00599                                 if (aux_port == 0) {
00600                                         filterAverageConfigPixels = false; //3x3
00601                                         continue;
00602                                 } else if (aux_port == 1) {
00603                                         filterAverageConfigPixels = true; //5x5
00604                                         continue;                       
00605                                 }
00606                         }
00607                         ROS_WARN("*invalid value for Average Filter Pixels Configuration");
00608                         return help();
00609                 }
00610                 else if( std::string(argv[i]) == "-avfite" ) {
00611                    if( sscanf(argv[++i], "%d", &averageConfigIters) != 1 
00612                                 || averageConfigIters < 1 || averageConfigIters > 255 ) {
00613                                 ROS_WARN("*invalid value for Average filter Iterations value");
00614                                 return help();
00615                         }
00616                 }
00617                 else if( std::string(argv[i]) == "-gafpix" ) {
00618                         if( sscanf(argv[++i], "%d", &aux_port) != 1 ) {
00619                                 if (aux_port == 0) {
00620                                         gaussConfigIters = false; //3x3
00621                                         continue;
00622                                 } else if (aux_port == 1) {
00623                                         gaussConfigIters = true; //5x5
00624                                         continue;                       
00625                                 }
00626                         }
00627                         ROS_WARN("*invalid value for Gauss Filter Pixels Configuration");
00628                         return help();
00629                 }
00630                 else if( std::string(argv[i]) == "-gafite" ) {
00631                    if( sscanf(argv[++i], "%d", &gaussConfigIters) != 1 
00632                                 || gaussConfigIters < 1 || gaussConfigIters > 255 ) {
00633                                 ROS_WARN("*invalid value for Gauss filter Iterations value");
00634                                 return help();
00635                         }
00636                 }
00637                 else if( std::string(argv[i]) == "-slacw" ) {
00638                    if( sscanf(argv[++i], "%d", &SLAConfigWindows) != 1 
00639                                 || SLAConfigWindows < 1 || SLAConfigWindows > 255 ) {
00640                                 ROS_WARN("*invalid value for SLAF Filter Windows size value");
00641                                 return help();
00642                         }
00643                 }
00644                 // additional parameters
00645                 else if( std::string(argv[i]) == "-af" ) {
00646                         amplitudeFilterOn = true;
00647                 }
00648                 else if( std::string(argv[i]) == "-at" ) {
00649                         if( sscanf(argv[++i], "%f", &amplitudeThreshold) != 1 
00650                                 || amplitudeThreshold < 0 || amplitudeThreshold > 2500 ) {
00651                                 ROS_WARN("*invalid amplitude threshold");
00652                                 return help();
00653                         }       
00654                 }
00655                 // print help
00656                 else if( std::string(argv[i]) == "--help" ) {
00657                         ROS_INFO_STREAM("argument: " << argc << " which: " << argv[i]);         
00658                         return help();
00659                 }
00660                 else if( argv[i][0] == '-' ) {
00661                         ROS_WARN_STREAM("invalid option " << argv[i]);
00662                         return help();
00663                 }
00664         }       
00665 
00666         ROS_INFO_STREAM("Connection data: tcp_ip: " << sentisConfig.tcp_ip << " tcp_port: " << sentisConfig.tcp_port << " udp_ip: " << sentisConfig.udp_ip << " udp_port: " << sentisConfig.udp_port );
00667 
00668         //connect with the device
00669         printf("CLIENT: open connection\n");
00670         handle = STSopen(&sentisConfig,&error);
00671         if(error != 0) {
00672             ROS_WARN_STREAM("Could not connect: " << error << "---------------");
00673             exit (1);
00674         }
00675 
00676         error = STSwriteRegister(handle, ImageDataFormat, XYZ_AMP_DATA );
00677         if(error != 0) {
00678             ROS_WARN_STREAM("Error writing register: " << error << "---------------");
00679             STSclose(handle);
00680             exit (1);
00681         }
00682 
00683         
00684         /*
00685          * ROS Node Initialization
00686          */
00687         pub_non_filtered = nh.advertise<PointCloud> ("depth_non_filtered", 1);
00688         pub_filtered = nh.advertise<PointCloud> ("depth_filtered", 1);
00689         return 1;
00690 }
00691 
00695 int publishData() {
00696 
00697          int size = 153600; // 120*4*160*2bytes
00698          short buffer[120*4][160];
00699 
00700         error = STSgetData(handle, &header,  (char *)buffer, &size, 0, 0);
00701         if (error == -1) {
00702                 ROS_ERROR_STREAM("Could get frame: " << error);
00703                 STSclose(handle);
00704                 return 0;
00705         }
00706 
00707         /*
00708          * Creating the pointcloud
00709          */
00710 
00711         // Fill in the cloud data
00712         PointCloud::Ptr msg_non_filtered (new PointCloud);
00713         msg_non_filtered->header.frame_id = "tf_sentis_tof";
00714         msg_non_filtered->height = header.imageHeight;
00715         msg_non_filtered->width = header.imageWidth;
00716         msg_non_filtered->is_dense = true;
00717         
00718         PointCloud::Ptr msg_filtered (new PointCloud);
00719         msg_filtered->header.frame_id = "tf_sentis_tof";
00720         msg_filtered->width    = 1;
00721         msg_filtered->height   = header.imageWidth*header.imageHeight;
00722         msg_filtered->is_dense = false;
00723 
00724         int countWidth=0;
00725 
00726         for (int i= 0 ; i < header.imageHeight; i++) {
00727                 for(int j=0; j<header.imageWidth; j++) {
00728                         pcl::PointXYZI temp_point;
00729                         temp_point.x = buffer[i+120][j]/1000.f;  //z
00730                         temp_point.y = buffer[i+(120*2)][j]/1000.f; // y
00731 
00732                         if (buffer[i][j]<0) {
00733                                 temp_point.z = nanf("");
00734                         } else {
00735                                 temp_point.z = buffer[i][j]/1000.f; //x
00736                         }
00737                         temp_point.intensity = buffer[i+(120*3)][j];
00738                         if(amplitudeFilterOn==true && buffer[i+(120*3)][j]>amplitudeThreshold) {
00739                                 msg_filtered->points.push_back(temp_point);
00740                                 countWidth++;
00741                         }
00742                         msg_non_filtered->points.push_back(temp_point);
00743                         //ROS_INFO_STREAM(temp_point);
00744                 }
00745         }
00746 
00747         msg_filtered->height   = countWidth;
00748         
00749          /*
00750           * Publishing the messages
00751           */
00752          if(amplitudeFilterOn){
00753                 #if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
00754                         msg_filtered->header.stamp = ros::Time::now().toNSec();
00755                 #else
00756                         msg_filtered->header.stamp = ros::Time::now();
00757                 #endif
00758                         pub_filtered.publish (msg_filtered);
00759          }
00760 
00761         #if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
00762                 msg_non_filtered->header.stamp = ros::Time::now().toNSec();
00763         #else
00764                 msg_non_filtered->header.stamp = ros::Time::now();
00765         #endif
00766                 pub_non_filtered.publish (msg_non_filtered);
00767 
00768         return 1;
00769 }
00770 
00779 int main(int argc, char *argv[]) {
00780         ROS_INFO("Starting sentis_tof_m100 ros...");
00781         ros::init (argc, argv, "sentis_tof_m100");
00782         ros::NodeHandle nh;
00783 
00784         dynamic_reconfigure::Server<sentis_tof_m100::sentis_tof_m100Config> srv;
00785         dynamic_reconfigure::Server<sentis_tof_m100::sentis_tof_m100Config>::CallbackType f;
00786 
00787         f = boost::bind(&callback, _1, _2);
00788 
00789         if(initialize(argc, argv,nh)){
00790                 first = true;
00791                 srv.setCallback(f);
00792                 first = false;
00793                 ROS_INFO("Initalized Camera... Reading Data");
00794                 ros::Rate loop_rate(10);
00795                 while (nh.ok())
00796                 {
00797                         publishData();
00798                         ros::spinOnce ();
00799                         loop_rate.sleep ();
00800                 }
00801         } else {
00802                 ROS_WARN("Cannot Initialize Camera. Check the parameters and try again!!");
00803                 return 0;
00804         }
00805 
00806         STSclose(handle);
00807         return 0;
00808 }


sentis_tof_m100
Author(s): Angel Merino , Simon Vogl
autogenerated on Mon Oct 6 2014 07:35:00