laser_data_load.c
Go to the documentation of this file.
00001 #include <ctype.h>
00002 
00003 #include "csm_all.h"
00004 #include "laser_data_drawing.h"
00005 
00007 int ld_read_some(FILE*file, LDP **array, int*num, int (*accept)(LDP));
00008 
00009         /* Every n scans */
00010         int interval_accept(LDP ld);
00011                 int interval_count = 0;
00012                 int interval_interval = 10;
00013 
00014         /* Every one */
00015         int always(LDP ld);
00016         
00017         /* Read according to distance */
00018         int distance_accept(LDP ld);
00019                 static int distance_count;
00020                 static double distance_last_pose[3];
00021                 static double distance_interval_xy = 10;
00022                 static double distance_interval_th = 10;
00023                 static ld_reference distance_reference;
00024                 void distance_accept_reset(ld_reference, double interval_xy, double interval_th);
00025 
00026 
00027 
00028 /* ---------------------------------------- */
00029 
00030 int ld_read_some(FILE*file, LDP **array, int*num, int (*accept)(LDP)) {
00031         *array = 0; *num = 0;
00032         int size = 10;
00033         LDP * ar = (LDP*) malloc(sizeof(LDP)*size);
00034         
00035         while(1) {
00036                 LDP ld = ld_read_smart(file);
00037 
00038                 if(!ld) break;
00039                 
00040                 if( ! (*accept)(ld) ) {
00041                         ld_free(ld);
00042                         continue;
00043                 }
00044                 
00045                 
00046                 ar[(*num)++] = ld;
00047                 
00048                 if(*num > size - 1) {
00049                         size *= 2;
00050                         if(! (ar = (LDP*) realloc(ar, sizeof(LDP)*size)) ) {
00051                                 sm_error("Cannot allocate (size=%d)\n", size);
00052                                 return 0;
00053                         }
00054                 }
00055         }
00056 
00057         *array = ar; 
00058 
00059         return feof(file);
00060 }
00061 
00062 
00063 /* Read every tot scans */
00064 int interval_accept(LDP ld) {
00065         ld=ld;
00066         int accept = interval_count % interval_interval == 0;
00067         interval_count++;
00068         return accept;
00069 }
00070 
00071 int ld_read_some_scans(FILE*file, LDP **array, int*num, int interval) {
00072         interval_count = 0;
00073         interval_interval = interval;
00074         return ld_read_some(file, array, num, interval_accept);
00075 }
00076 
00077 /* Read all scans */
00078 int always(LDP ld)  { ld=ld; return 1; }
00079 int ld_read_all(FILE*file, LDP **array, int*num) {
00080         return ld_read_some(file, array, num, always);
00081 }
00082 
00083 void distance_accept_reset(ld_reference which, double interval_xy, double interval_th) {
00084         distance_count = 0;
00085         distance_interval_xy = interval_xy;
00086         distance_interval_th = interval_th;
00087         distance_reference = which;
00088 }
00089 
00090 int distance_accept(LDP ld) {
00091         double * this_pose = ld_get_reference_pose(ld, distance_reference);
00092         if(!this_pose) return 0;
00093         
00094         distance_count++;
00095         if(distance_count == 1) {
00096                 copy_d(this_pose, 3, distance_last_pose);
00097                 return 1;
00098         } else {
00099                 double diff[3];
00100                 pose_diff_d(distance_last_pose, this_pose, diff);
00101                 double distance  = norm_d(diff);
00102                 
00103                 if(distance >= distance_interval_xy || 
00104                    fabs(diff[2]) >= distance_interval_th ) 
00105                 {
00106                         copy_d(this_pose, 3, distance_last_pose);
00107                 /*      sm_debug("Accepting #%d, %f\n", distance_count, distance);*/
00108                         return 1;
00109                 }
00110                 else 
00111                         return 0;
00112         }
00113 }
00114 
00115 int ld_read_some_scans_distance(FILE*file, LDP **array, int*num, 
00116         ld_reference which, double d_xy, double d_th) {
00117         distance_accept_reset(which, d_xy, d_th);
00118         return ld_read_some(file, array, num, distance_accept);
00119 }
00120 
00121 
00122 
00130 LDP ld_read_smart(FILE*f) {
00131         while(1) {
00132                 int c;
00133                 while(1) {
00134                         c = fgetc(f);
00135                         if(feof(f)) { 
00136                                 /* sm_debug("eof\n"); */
00137                                 return 0;
00138                         }
00139                         if(!isspace(c)) break;
00140                 }
00141                 ungetc(c, f);
00142 
00143                 switch(c) {
00144                         case '{': {
00145 /*                              sm_debug("Reading JSON\n"); */
00146                                 return ld_from_json_stream(f);
00147                         }
00148                         case 'F': {
00149 /*                              sm_debug("Reading Carmen\n");  */
00150                                 LDP ld;
00151                                 if(!ld_read_next_laser_carmen(f, &ld)) {
00152                                         sm_error("bad carmen\n");
00153                                         return 0;
00154                                 }
00155                                 return ld;
00156                         }
00157                         default: {
00158                                 /*sm_error("Could not read ld. First char is '%c'. ", c);*/
00159                                 char max_line[10000];
00160                                 char * res = fgets(max_line, 10000-2, f);
00161                                 if(!res) {
00162                                         sm_error("Could not skip line. \n");
00163                                         return 0;
00164                                 } else {
00165                                         fprintf(stderr, "s");
00166 /*                                      sm_error("Skipped '%s'\n", res);*/
00167                                 }
00168                         }
00169                 }
00170         }
00171 }
00172 
00173 LDP ld_read_smart_string(const char*line) {
00174         switch(*line) {
00175                 case '{': 
00176                         return ld_from_json_string(line);
00177                 
00178                 case 'F': 
00179                         return ld_from_carmen_string(line);
00180                         
00181                 default:
00182                         sm_error("Invalid laserdata format: '%s'.", line);
00183                         return 0;
00184         }
00185 }
00186 
00187 
00188 
00189 


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