main.cc
Go to the documentation of this file.
00001 
00023 #include <string>
00024 #include <iostream>
00025 #include <signal.h>
00026 #include <stdlib.h>
00027 #include <sicktoolbox/SickLD.hh>
00028 #include <stdlib.h>
00029 #include "ConfigFile.h"
00030 
00031 #define INVALID_OPTION_STRING        "   Invalid option!!! :o(" 
00032 #define PROMPT_STRING                                   "ld?> " 
00033 
00034 /* Config file parameters */
00035 #define CONFIG_OPT_MOTOR_SPD_STR          "SICK_LD_MOTOR_SPEED"
00036 #define CONFIG_OPT_SCAN_AREA_STR           "SICK_LD_SCAN_AREAS"
00037 #define CONFIG_OPT_SCAN_RES_STR       "SICK_LD_SCAN_RESOLUTION"
00038 
00039 using namespace std;
00040 using namespace SickToolbox;
00041 
00047 void sigintHandler(int signal);
00048 
00055 int getUserOption(bool &is_null_input);
00056 
00062 string getFilename();
00063 
00069 void setConfig();
00070 
00075 void printConfig();
00076 
00095 bool parseScanAreasStr(string& scan_areas_str, double *start_angs, double *stop_angs, int& num_sectors);
00096 
00105 bool parseNumStr(const string& entry, double& num);
00106 
00107 /* A pointer to the current driver instance */
00108 SickLD *sick_ld = NULL;
00109 
00110 int main(int argc, char* argv[])
00111 {
00112   
00113   string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS);  // IP address of the Sick LD unit
00114 
00115   /* Check the num args */
00116   if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
00117     cerr << "Usage: ld_config [SICK IP ADDRESS]" << endl
00118               << "Ex. ld_config 192.168.1.11" << endl;
00119     return -1;
00120   }
00121   
00122   /* Assign the IP address */
00123   if(argc == 2) {
00124     sick_ip_addr = argv[1];
00125   }
00126 
00127   /* Instantiate the SickLD driver */
00128   sick_ld = new SickLD(sick_ip_addr);
00129 
00130   cout << endl;
00131   cout << "The Sick LIDAR C++/Matlab Toolbox    " << endl;
00132   cout << "Sick LD Config Utility          " << endl;
00133   cout << endl;
00134 
00135   /* Initialize the Sick LD */
00136   try {
00137     sick_ld->Initialize();
00138   }
00139 
00140   catch(...) {
00141     cerr << "Initialize failed! Are you using the correct IP address?" << endl;
00142     return -1;
00143   }
00144 
00145   /* Register the signal handler */
00146   signal(SIGINT,sigintHandler);
00147 
00148   do {
00149 
00150     cout << "Enter your choice: (Ctrl-c to exit)" << endl;
00151     cout << "  [1] Set new configuration"<< endl;
00152     cout << "  [2] Show current settings"<< endl;
00153     cout << PROMPT_STRING;
00154     
00155     bool is_null_input;
00156     switch(getUserOption(is_null_input)) {
00157 
00158     case 1:
00159       setConfig();
00160       break;
00161     case 2:
00162       printConfig();
00163       break;
00164     default:    
00165       if(!is_null_input) {
00166         cerr << INVALID_OPTION_STRING << endl;
00167       }
00168 
00169     }
00170     
00171     cout << endl;
00172     
00173   } while(true);
00174   
00175   /* Success */
00176   return 0;
00177   
00178 }
00179 
00183 void sigintHandler(int signal) {
00184 
00185   cout << endl;
00186   cout << "Quitting..." << endl;
00187 
00188   /* Unitialize the device */
00189   try {
00190 
00191     sick_ld->Uninitialize();
00192     delete sick_ld;
00193 
00194   }
00195   
00196   catch(...) {
00197     cerr << "Uninitialize failed!" << endl;
00198     exit(-1);
00199   }
00200 
00201   cout << endl;
00202   cout << "Thanks for using the Sick LIDAR Matlab/C++ Toolbox!" << endl;
00203   cout << "Bye Bye :o)" << endl;
00204 
00205   exit(0);
00206   
00207 }
00208 
00212 int getUserOption(bool &is_null_input) {
00213 
00214   string user_input_str;
00215   getline(cin,user_input_str);
00216 
00217   // Check whether its null input
00218   is_null_input = true;
00219   if (user_input_str.length() > 0) {
00220     is_null_input = false;
00221   }
00222 
00223   int int_val = 0;
00224   istringstream input_stream(user_input_str);
00225   input_stream >> int_val;
00226 
00227   return int_val;
00228   
00229 }
00230 
00234 string getFilename() {
00235 
00236   string filename;
00237 
00238   cout << "config file: ";
00239   getline(cin,filename);
00240 
00241   return filename;
00242   
00243 }
00244 
00248 void setConfig() {
00249 
00250   int motor_spd;
00251   int num_sectors;
00252   double scan_res;
00253   double start_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0};
00254   double stop_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0};
00255   string scan_areas_str;
00256   
00257   ConfigFile sick_config_file;   // Extracts values from config file
00258   string config_path;
00259 
00260   /* Prompt the user for the filename */
00261   config_path = getFilename();
00262 
00263   /* Instantiate the parser */
00264   if(ifstream(config_path.c_str()) != NULL) {
00265     sick_config_file = ConfigFile(config_path);
00266   }
00267   else {
00268     cerr << "Invalid filename!" << endl;
00269     return;
00270   }
00271 
00272   /* Use the ConfigFile class to extract the various parameters for
00273    * the sick configuration.
00274    *
00275    * NOTE: The third parameter specifies the value to use, if the second 
00276    *       parameter is not found in the file.
00277    */
00278   if(!sick_config_file.readInto(motor_spd,CONFIG_OPT_MOTOR_SPD_STR)) {
00279     cerr << "ERROR: Invalid config file - " << CONFIG_OPT_MOTOR_SPD_STR << " needs to be specified!" << endl;
00280     return;
00281   }
00282   
00283   if(!sick_config_file.readInto(scan_res,CONFIG_OPT_SCAN_RES_STR)) {
00284     cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_RES_STR << " needs to be specified!" << endl;
00285     return;
00286   }
00287 
00288   if(!sick_config_file.readInto(scan_areas_str,CONFIG_OPT_SCAN_AREA_STR)) {
00289     cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_AREA_STR << " needs to be specified!" << endl;
00290     return;
00291   }
00292 
00293   /* Extract the start/stop pairs and angle res for the scanning sectors */
00294   if (!parseScanAreasStr(scan_areas_str,start_angs,stop_angs,num_sectors)) {
00295     cerr << "ERROR: Parser failed to extract scan areas!" << endl;
00296     return;
00297   }
00298 
00299   /* Set the global parameters - we pass them all at once to ensure a feasible config */
00300   cout << endl << "\tAttempting to configure the Sick LD..." << endl;
00301       
00302   try {
00303     sick_ld->SetSickGlobalParamsAndScanAreas(motor_spd,scan_res,start_angs,stop_angs,num_sectors);
00304   }
00305 
00306   catch(SickConfigException &sick_config_exception) {
00307     cerr << "ERROR: Couldn't set requested configuration!" << endl;
00308     return;
00309   }
00310   
00311   catch(...) {
00312     exit(-1);
00313   }
00314   
00315   cout << "\t\tConfiguration Successfull!!!" << endl;
00316   
00317 }
00318 
00322 void printConfig() {
00323 
00324   cout << endl;
00325   sick_ld->PrintSickStatus();
00326   sick_ld->PrintSickIdentity();
00327   sick_ld->PrintSickGlobalConfig();
00328   sick_ld->PrintSickEthernetConfig();
00329   sick_ld->PrintSickSectorConfig();
00330   
00331 }
00332 
00336 bool parseScanAreasStr(string& areas, double * start_angs, double * stop_angs, int& num_pairs) {
00337   
00338   unsigned long i;              // number of sectors found so far
00339   unsigned int start_pos = 0;   // starting position of a sector in 'areas'
00340   unsigned int end_pos = 0;     // ending position of a sector in 'areas'
00341   unsigned int split = 0;       // position of the delimiter inside a sector
00342   string pair;                  // a string of 'areas' that contains one start and one stop angle
00343 
00344   /* Get the beginning and end of the first scan sector */
00345   start_pos = areas.find('[',start_pos); // Find the next [, starting at start_pos
00346   end_pos = areas.find(']',start_pos);
00347 
00348   /* Keep getting sectors until we either run out of open
00349    * brackets or we exceed the number of allowed sectors
00350    */
00351   for(i=0; (start_pos != (unsigned int)string::npos) && (i <= SickLD::SICK_MAX_NUM_MEASURING_SECTORS); i++) {
00352 
00353     pair = areas.substr(start_pos+1,end_pos-(start_pos+1));
00354 
00355     /* Eliminate any padding before first value */
00356     try {      
00357       pair = pair.substr(pair.find_first_not_of(' '));    
00358     }
00359 
00360     catch(...) {
00361       cerr << "ERROR: There was an problem parsing your scan areas! Check your config file." << endl;
00362       return false;
00363     }
00364 
00365     split = pair.find(' ');
00366 
00367     /* Catch a lack of ' ' inside the sector definition;
00368      * alternative is to let it fail at pair.substr(split)
00369      */
00370     if(split == (unsigned int)string::npos) {
00371       cerr << "ERROR: Invalid sector definition." << endl;
00372       return false;
00373     }
00374 
00375     /* Get the number from the beginning to the delimiter */
00376     if(!parseNumStr(pair.substr(0,split),start_angs[i])) {
00377       cerr << "ERROR: Invalid start angle found." << endl;
00378       return false;
00379     }
00380 
00381     /* Get the number from the delimiter to the end */
00382     if(!parseNumStr(pair.substr(split),stop_angs[i])) {
00383       cerr << "ERROR: Invalid stop angle found." << endl;
00384       return false;
00385     }
00386 
00387     start_pos = end_pos; // Shift to the end of the current sector
00388 
00389     /* Try to find another sector */
00390     start_pos = areas.find("[", start_pos);
00391     end_pos = areas.find("]", start_pos);
00392 
00393   }
00394 
00395   num_pairs = i;
00396 
00397   /* Check if we broke out because of too many loops */
00398   if(num_pairs > SickLD::SICK_MAX_NUM_MEASURING_SECTORS) {
00399     cerr << "ERROR: Too many scan areas found (max " << SickLD::SICK_MAX_NUM_MEASURING_SECTORS << ")" << endl;
00400     return false;
00401   }
00402 
00403   /* Or, we might not have even entered the loop */
00404   if(num_pairs == 0) {
00405     cerr << "ERROR: No scan areas found! Check brackets in your config file." << endl;
00406     return false;
00407   }
00408 
00409   /* Success! */
00410   return true;
00411   
00412 }
00413 
00417 bool parseNumStr(const string& entry, double& num)
00418 {
00419 
00420   string num_str = entry.substr(entry.find_first_not_of(' '));  
00421   istringstream input_stream(num_str.c_str());  
00422   if(!(input_stream >> num)) {
00423     cerr << "ERROR: Invalid angle value: " + num_str << endl;
00424     return false;
00425   }
00426 
00427   /* Success! */
00428   return true;
00429 
00430 }


sicktoolbox
Author(s): Jason Derenick , Thomas Miller
autogenerated on Sun May 5 2019 02:28:23