argos3d_p100_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 #define SOURCE_PARAM ""
00043 #define PROC_PARAM ""
00044 #include <pmdsdk2.h>
00045 
00046 #include <ros/console.h>
00047 #include <ros/ros.h>
00048 #include <tf/transform_listener.h>
00049 #include <ros/publisher.h>
00050 #include <pcl_ros/point_cloud.h>
00051 #include <pcl/point_types.h>
00052 #include <pcl/io/pcd_io.h>
00053 #include <pcl/filters/statistical_outlier_removal.h>
00054 
00055 #include <stdio.h>
00056 #include <time.h>
00057 #include <sstream>
00058 
00059 #include <argos3d_p100/argos3d_p100Config.h>
00060 #include <dynamic_reconfigure/server.h>
00061 
00062 typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
00063 
00071 int integrationTime;
00072 int modulationFrequency;
00073 int frameRate;
00074 bool bilateralFilter;
00075 int flip_x, flip_y;
00076 
00077 bool AmplitudeFilterOn;
00078 float AmplitudeThreshold;
00079 
00080 int noOfRows;
00081 int noOfColumns;
00082 
00083 bool first;
00087 PMDHandle hnd;
00088 int res;
00089 char err[128];
00090 
00095 bool dataPublished;
00096 ros::Publisher pub_non_filtered;
00097 ros::Publisher pub_filtered;
00098 
00105 int help() {
00106         std::cout << "\n Using help for argos3d_p100_ros_pkg\n"
00107                 " You can set default configuration values for the camera with the following options: \n" << std::endl;
00108         std::cout << " Usage:\n rosrun argos3d_p100 argos3d_p100_node "<< std::endl
00109                 << "\t-it *Integration_Time* \n\tIntegration time(in msec) for the sensor \n\t(min: 100 | max: 2700 | default: 1500) "<< std::endl
00110                 << "\t-mf  *Modulation_Frequency* \n\tSet the modulation frequency(Hz) of the sensor \n\t(min: 5000000 | max: 30000000 | default: 30000000) "<< std::endl
00111                 << "\t-bf *Bilateral_Filter* \n\tTurns bilateral filtering on or off \n\t(ON: if set | OFF: default) "<< std::endl
00112                 << "\t-fr *Frame_Rate* \n\tSet the frame rate of the camera by setting the Phase Time (Please be careful when setting values higher than 40 FPS without using an extra cooling system. The camera can stress by overheating and be damaged). \n\t(min: 1 | max: 160 | default: 40)" << std::endl
00113                 << "\t-flip_x *flip_x* \n\tFlip images in the x coordinate. \n\t(ON: if set | OFF: default)" << std::endl
00114                 << "\t-flip_y *flip_y* \n\tFlip images in the y coordinate. \n\t(ON: if set | OFF: default)" << std::endl
00115                 << "\t-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
00116                 << "\t-at *Amplitude_Threshold* \n\tWhat should be the amplitude filter threshold. Image pixels with lesser 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
00117                 << "\n Example:" << std::endl
00118                 << "rosrun argos3d_p100 argos3d_p100_node -it 1500 -mf 30000000 \n" << std::endl;
00119         exit(0);
00120 } //print_help
00121 
00131 void callback(argos3d_p100::argos3d_p100Config &config, uint32_t level)
00132 {
00133         // Check the configuretion parameters with those given in the initialization
00134         if(first) {
00135                 config.Integration_Time = integrationTime;
00136                 config.Modulation_Frequency = modulationFrequency;
00137                 config.Frame_rate = frameRate;
00138                 config.Bilateral_Filter = !bilateralFilter;
00139                 
00140                 if (flip_x == -1)
00141                         config.Flip_X = true;
00142                 if (flip_y == -1)
00143                         config.Flip_Y = true;   
00144 
00145                 config.Amplitude_Filter_On = AmplitudeFilterOn;
00146                 config.Amplitude_Threshold = AmplitudeThreshold;
00147                 
00148                 integrationTime = modulationFrequency = frameRate = -1;
00149         }
00150 
00151         if(integrationTime != config.Integration_Time) {
00152                 integrationTime = config.Integration_Time;
00153                 res = pmdSetIntegrationTime (hnd, 0, integrationTime);
00154                 if (res != PMD_OK)
00155                 {
00156                         pmdGetLastError (hnd, err, 128);
00157                         ROS_WARN_STREAM("Could not set integration time: " << err);
00158                 }
00159         }
00160 
00161         if(modulationFrequency != config.Modulation_Frequency) {
00162                 modulationFrequency = config.Modulation_Frequency;
00163                 res = pmdSetModulationFrequency(hnd, 0, modulationFrequency);
00164                 if (res != PMD_OK) {
00165                         pmdGetLastError (hnd, err, 128);
00166                         ROS_WARN_STREAM("Could not set modulation frequency: " << err);
00167                 }
00168         }
00169         
00170         if(frameRate != config.Frame_rate) {
00171                 frameRate = config.Frame_rate;
00172                 err[0] = 0;
00173                 std::stringstream spt;
00174                 spt << "SetPhaseTime " << (1000000/(4*frameRate));
00175                 res = pmdSourceCommand (hnd, err, sizeof(err), spt.str().c_str());
00176                 if (res != PMD_OK) {
00177                         pmdGetLastError (hnd, err, 128);
00178                         ROS_WARN_STREAM("Could not set frame rate: " << err);
00179                 }
00180         }
00181 
00182         if(bilateralFilter != config.Bilateral_Filter) {
00183                 bilateralFilter = config.Bilateral_Filter;
00184                 err[0] = 0;
00185                 if(bilateralFilter)
00186                         res = pmdProcessingCommand(hnd, err, sizeof(err), "SetBilateralFilter on");
00187                 else
00188                         res = pmdProcessingCommand(hnd, err, sizeof(err), "SetBilateralFilter off");
00189                 if (res != PMD_OK) {
00190                         pmdGetLastError (hnd, err, 128);
00191                         ROS_WARN_STREAM("Could not set bilateral filter: " << err);
00192                 }
00193         }
00194 
00195         flip_x = flip_y = 1;
00196         if (config.Flip_X)
00197                 flip_x = -1;
00198         if (config.Flip_Y)
00199                 flip_y = -1;
00200 
00201         AmplitudeFilterOn = config.Amplitude_Filter_On;
00202         AmplitudeThreshold = config.Amplitude_Threshold;
00203 }
00204 
00214 int initialize(int argc, char *argv[],ros::NodeHandle nh){
00215         /*
00216          * Inital Setup for parameters
00217          */
00218         integrationTime = 1500;
00219         modulationFrequency = 30000000;
00220         frameRate = 40;
00221         bilateralFilter = false;
00222         
00223         flip_x = flip_y = 1;
00224         
00225         AmplitudeFilterOn = false;
00226         AmplitudeThreshold = 0;
00227 
00228         for( int i = 1; i < argc; i++) {
00229                 // reading width
00230                 if( std::string(argv[i]) == "-it" ) {
00231                         if( sscanf(argv[++i], "%d", &integrationTime) != 1 
00232                                 || integrationTime < 100 || integrationTime > 2700 ) {
00233                                 ROS_WARN("*invalid integration time");
00234                                 return help();
00235                         }
00236                 }
00237                 // reading heigth
00238                 else if( std::string(argv[i]) == "-mf" ) {
00239                         if( sscanf(argv[++i], "%d", &modulationFrequency) != 1 
00240                                 || modulationFrequency < 5000000 || modulationFrequency > 30000000 ) {
00241                                 ROS_WARN("*invalid modulation frequency");
00242                                 return help();
00243                         }
00244                 }
00245                 if( std::string(argv[i]) == "-fr" ) {
00246                         if( sscanf(argv[++i], "%d", &frameRate) != 1 
00247                                 || frameRate < 1 || frameRate > 40 ) {
00248                                 ROS_WARN("*invalid frame rate");
00249                                 return help();
00250                         }
00251                 } else if( std::string(argv[i]) == "-bf" ) {
00252                         bilateralFilter = true;
00253                 } else if( std::string(argv[i]) == "-flip_x" ) {
00254                         flip_x = -1;
00255                 } else if( std::string(argv[i]) == "-flip_y" ) {
00256                         flip_y = -1;
00257                 }
00258                 // additional parameters
00259                 else if( std::string(argv[i]) == "-af" ) {
00260                         AmplitudeFilterOn = true;
00261                 }
00262                 else if( std::string(argv[i]) == "-at" ) {
00263                         if( sscanf(argv[++i], "%f", &AmplitudeThreshold) != 1 
00264                                 || AmplitudeThreshold < 0 || AmplitudeThreshold > 2500 ) {
00265                                 ROS_WARN("*invalid amplitude threshold");
00266                                 return help();
00267                         }       
00268                 }
00269                 // print help
00270                 else if( std::string(argv[i]) == "--help" ) {
00271                         ROS_WARN_STREAM("arguments: " << argc << " which: " << argv[i]);                
00272                         return help();
00273                 }
00274                 else if( argv[i][0] == '-' ) {
00275                         ROS_WARN_STREAM("invalid option " << argv[i]);
00276                         return help();
00277                 }
00278         }       
00279 
00280         /*
00281          * Camera Initialization
00282          */
00283         std::stringstream sourcePluginLocation, procPluginLocation;
00284         sourcePluginLocation.clear();
00285         procPluginLocation.clear();
00286         sourcePluginLocation << PMD_PLUGIN_DIR << "digicam";
00287         procPluginLocation << PMD_PLUGIN_DIR << "digicamproc";
00288 
00289         // If the camera is not connected at all, we will get an segmentation fault.
00290         res = pmdOpen (&hnd, sourcePluginLocation.str().c_str(), SOURCE_PARAM, procPluginLocation.str().c_str(), PROC_PARAM);
00291         if (res != PMD_OK)
00292         {
00293                 pmdGetLastError (0, err, 128);
00294                 ROS_ERROR_STREAM("Could not connect: " << err);
00295                 return 0;
00296         }
00297 
00298         char result[128];
00299         result[0] = 0;
00300         res = pmdSourceCommand(hnd, result, sizeof(result), "IsCalibrationDataLoaded");
00301         if (res != PMD_OK)
00302         {
00303                 pmdGetLastError (0, err, 128);
00304                 ROS_ERROR_STREAM("Could not execute source command: " << err);
00305                 pmdClose (hnd);
00306                 return 0;
00307         }
00308         if (std::string(result) == "YES")
00309                 ROS_INFO("Calibration file loaded.");
00310         else
00311                 ROS_INFO("No calibration file found");
00312 
00313         res = pmdUpdate (hnd);
00314         if (res != PMD_OK)
00315         {
00316                 pmdGetLastError (hnd, err, 128);
00317                 ROS_ERROR_STREAM("Could not transfer data: " << err);
00318                 pmdClose (hnd);
00319                 return 0;
00320         }
00321 
00322         PMDDataDescription dd;
00323 
00324         res = pmdGetSourceDataDescription (hnd, &dd);
00325         if (res != PMD_OK)
00326         {
00327                 pmdGetLastError (hnd, err, 128);
00328                 ROS_ERROR_STREAM("Could not get data description: " << err);
00329                 pmdClose (hnd);
00330                 return 0;
00331         }
00332 
00333         if (dd.subHeaderType != PMD_IMAGE_DATA)
00334         {
00335                 ROS_ERROR_STREAM("Source data is not an image!\n");
00336                 pmdClose (hnd);
00337                 return 0;
00338         }
00339         noOfRows = dd.img.numRows;
00340         noOfColumns = dd.img.numColumns;
00341 
00342         /*
00343          * ROS Node Initialization
00344          */
00345         pub_non_filtered = nh.advertise<PointCloud> ("depth_non_filtered", 1);
00346         pub_filtered = nh.advertise<PointCloud> ("depth_filtered", 1);
00347         dataPublished=true;
00348         return 1;
00349 }
00350 
00351 static float * cartesianDist = 0;
00352 static float * amplitudes = 0;
00353 
00354 
00360 int publishData() {
00361 
00362         /*
00363          * Update Camera settings
00364          */
00365         res = pmdUpdate (hnd);
00366         if (res != PMD_OK)
00367         {
00368                 pmdGetLastError (hnd, err, 128);
00369                 ROS_ERROR_STREAM("Could transfer data: " << err);
00370                 pmdClose (hnd);
00371                 return 0;
00372         }
00373 
00374         /*
00375          * Obtain PointClouds
00376          */
00377         if (!cartesianDist)
00378                 cartesianDist = new float [noOfRows * noOfColumns * 3];
00379         res = pmdGet3DCoordinates (hnd, cartesianDist, noOfColumns * noOfRows * 3 * sizeof (float));
00380         if (res != PMD_OK)
00381         {
00382                 pmdGetLastError (hnd, err, 128);
00383                 ROS_ERROR_STREAM("Could get cartesian coordinates: " << err);
00384                 pmdClose (hnd);
00385                 return 0;
00386         }
00387 
00388 
00389         /*
00390          * Obtain Amplitude Values
00391          */
00392         if (!amplitudes)
00393                 amplitudes = new float [noOfRows * noOfColumns];
00394 
00395         res = pmdGetAmplitudes (hnd, amplitudes, noOfRows * noOfColumns * sizeof (float));
00396         if (res != PMD_OK)
00397         {
00398                 pmdGetLastError (hnd, err, 128);
00399                 ROS_ERROR_STREAM("Could get amplitude values: " << err);
00400                 pmdClose (hnd);
00401                 return 1;
00402         }
00403 
00404         /*
00405          * Creating the pointcloud
00406          */
00407          
00408         // Fill in the cloud data
00409         PointCloud::Ptr msg_non_filtered (new PointCloud);
00410         msg_non_filtered->header.frame_id = "tf_argos3d";
00411         msg_non_filtered->height = 1;
00412         msg_non_filtered->width = noOfRows*noOfColumns;
00413         
00414         PointCloud::Ptr msg_filtered (new PointCloud);
00415         msg_filtered->header.frame_id = "tf_argos3d";
00416         msg_filtered->width    = 1;
00417         msg_filtered->height   = noOfColumns*noOfRows;
00418         msg_filtered->is_dense = false;
00419         //msg_filtered->points.resize (noOfRows*noOfColumns);
00420 
00421         int countWidth=0;
00422 
00423         for (size_t i = 0; i < noOfRows*noOfColumns; ++i)       {
00424                 pcl::PointXYZI temp_point;
00425                 temp_point.x = cartesianDist[(i*3) + 0]*flip_x;
00426                 temp_point.y = cartesianDist[(i*3) + 1]*flip_y;
00427                 temp_point.z = cartesianDist[(i*3) + 2];
00428                 temp_point.intensity = amplitudes[i];
00429 
00430                 if(AmplitudeFilterOn==true && amplitudes[i]>AmplitudeThreshold) {
00431                         msg_filtered->points.push_back(temp_point);
00432                         countWidth++;
00433                 }
00434                 msg_non_filtered->points.push_back(temp_point);
00435         }
00436         msg_filtered->height   = countWidth;
00437 
00438          /*
00439           * Publishing the messages
00440           */
00441          if(AmplitudeFilterOn){
00442                 #if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
00443                         msg_filtered->header.stamp = ros::Time::now().toNSec();
00444                 #else
00445                         msg_filtered->header.stamp = ros::Time::now();
00446                 #endif
00447                         pub_filtered.publish (msg_filtered);
00448          }
00449 
00450         #if ROS_VERSION > ROS_VERSION_COMBINED(1,9,49)
00451                 msg_non_filtered->header.stamp = ros::Time::now().toNSec();
00452         #else
00453                 msg_non_filtered->header.stamp = ros::Time::now();
00454         #endif
00455                 pub_non_filtered.publish (msg_non_filtered);
00456 
00457         return 1;
00458 }
00459 
00468 int main(int argc, char *argv[]) {
00469         ROS_INFO("Starting argos3d_p100 ros...");
00470         ros::init (argc, argv, "argos3d_p100");
00471         ros::NodeHandle nh;
00472 
00473         dynamic_reconfigure::Server<argos3d_p100::argos3d_p100Config> srv;
00474         dynamic_reconfigure::Server<argos3d_p100::argos3d_p100Config>::CallbackType f;
00475 
00476         f = boost::bind(&callback, _1, _2);
00477 
00478         if(initialize(argc, argv,nh)){
00479                 first = true;
00480                 srv.setCallback(f);
00481                 first = false;
00482                 ROS_INFO("Initalized Camera... Reading Data");
00483                 ros::Rate loop_rate(10);
00484                 while (nh.ok() && dataPublished)
00485                 {
00486                         if(publishData()) dataPublished==true;
00487                         ros::spinOnce ();
00488                         loop_rate.sleep ();
00489                 }
00490         } else {
00491                 ROS_WARN("Cannot Initialize Camera. Check the parameters and try again!!");
00492                 return 0;
00493         }
00494 
00495         pmdClose (hnd);
00496         return 0;
00497 }
00498 


argos3d_p100
Author(s): Angel Merino , Simon Vogl
autogenerated on Sun Oct 5 2014 22:20:26