ld_exp_tro1.c
Go to the documentation of this file.
00001 #include <string.h>
00002 #include <gsl/gsl_rng.h>
00003 #include <gsl/gsl_randist.h>
00004 #include <gsl/gsl_math.h>
00005 #include <math.h>
00006 #include <options/options.h>
00007 
00008 #include "../csm/csm_all.h"
00009 
00010 struct ld_exp_tro1_params {
00011         int seed;
00012 
00013         double max_xy_error;
00014         double max_theta_error_deg;
00015         
00016         const char* file_input;
00017         const char* file_output1;
00018         const char* file_output2;
00019         
00020         int num_per_scan;
00021         
00022         int debug;
00023 };
00024 
00025 const char * banner = 
00026         "This program prepares the data for one of the experiments. \n\n"
00027                 "The input is any sensor log (Carmen or JSON format) \n"
00028                 "The output are two files that contain laser_ref and laser_sens\n"
00029                 "(you have to match the i-th scan in the first file with the i-th\n"
00030                 " in the second).\n\n"
00031                 "The two files contain exactly the same date but for the 'odometry' field\n"
00032                 "The odometry error is uniform in the intervals given.\n";
00033 
00034 int main(int argc, const char ** argv) {
00035         sm_set_program_name(argv[0]);
00036         
00037         struct ld_exp_tro1_params p;
00038         
00039         options_banner(banner);
00040         
00041         struct option* ops = options_allocate(10);
00042         options_double(ops, "max_xy_error", &p.max_xy_error, 10.0, "Maximum error for x,y (m)");
00043         options_double(ops, "max_theta_error_deg", &p.max_theta_error_deg, 10.0, "Maximum error for orientation (deg)");
00044         options_int   (ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
00045 
00046         options_int(ops, "num_per_scan", &p.num_per_scan, 10, "Number of trials for each scan.");
00047 
00048         options_string(ops, "in", &p.file_input, "stdin", "Input file ");
00049         options_string(ops, "out1", &p.file_output1, "stdout", "Output file for first scan");
00050         options_string(ops, "out2", &p.file_output2, "stdout", "Output file for second scan");
00051         
00052         options_int(ops, "debug", &p.debug, 0, "Shows debug information");
00053         
00054         if(!options_parse_args(ops, argc, argv)) {
00055                 options_print_help(ops, stderr);
00056                 return -1;
00057         }
00058         
00059         sm_debug_write(p.debug);
00060 
00061         gsl_rng_env_setup();
00062         gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
00063         if(p.seed != 0)
00064         gsl_rng_set(rng, (unsigned int) p.seed);
00065         
00066         /* Open the two output files (possibly the same one) */
00067         
00068         FILE * in = open_file_for_reading(p.file_input);
00069         if(!in) return -3;
00070 
00071         FILE * out1 = open_file_for_writing(p.file_output1);
00072         if(!out1) return -2;
00073         
00074         FILE * out2;
00075         if(!strcmp(p.file_output1, p.file_output2)) {
00076                 out1 = out2;
00077         } else {
00078                 out2 = open_file_for_writing(p.file_output2);
00079                 if(!out2) return -2;
00080         }
00081 
00082         /* Read laser data from input file */
00083         LDP ld; int count=0;
00084         while( (ld = ld_read_smart(in))) {
00085                 count++;
00086                 if(!ld_valid_fields(ld))  {
00087                         sm_error("Invalid laser data (#%d in file)\n", count);
00088                         continue;
00089                 }
00090                 
00091                 for(int n=0; n < p.num_per_scan; n++) {                                 
00092                         ld->true_pose[0] = 0;
00093                         ld->true_pose[1] = 0;
00094                         ld->true_pose[2] = 0;
00095                         
00096                         ld->odometry[0] = 0;
00097                         ld->odometry[1] = 0;
00098                         ld->odometry[2] = 0;
00099                         
00100                         ld_write_as_json(ld, out1);
00101 
00102                         ld->odometry[0] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error;
00103                         ld->odometry[1] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error;
00104                         ld->odometry[2] = 2*(gsl_rng_uniform(rng)-0.5) * deg2rad(p.max_theta_error_deg);
00105                         
00106                         ld_write_as_json(ld, out2);
00107                 }
00108 
00109                 ld_free(ld);
00110         }
00111         
00112         return 0;
00113 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29