laser_data_json.c
Go to the documentation of this file.
00001 #include <errno.h>
00002 #include <string.h>
00003 #include <math.h>
00004 #include <ctype.h>
00005 
00006 #include "csm_all.h"
00007 
00008 void jo_add_timestamp(JO jo, const char*name, struct timeval*tv);
00009 int is_all_nan(const double *v, int n );
00010 void jo_add_double_array_if_not_nan(JO jo, const char*name, const double *v, int n);
00011 int all_is(int *a, int n, int v);
00012 
00013 /* -------------------------------------------- */
00014 
00015 
00016 JO matrix_to_json(gsl_matrix*m) {
00017         JO jo = jo_new_array();
00018         if(m->size1>1) {
00019                 size_t i,j;
00020                 for(i=0;i<m->size1;i++) {
00021                         JO row  = json_object_new_array();
00022                         for(j=0;j<m->size2;j++) {
00023                                 double v = gsl_matrix_get(m,i,j);
00024                                 json_object_array_add(row, jo_double_or_null(v));
00025                         }
00026                         json_object_array_add(jo, row);
00027                 }
00028         } else {
00029                 size_t i = 1, j;
00030                 JO row  = jo;
00031                 for(j=0;j<m->size2;j++) {
00032                         double v = gsl_matrix_get(m,i,j);
00033                         json_object_array_add(row, jo_double_or_null(v));
00034                 }
00035         }
00036         return jo;
00037 }
00038 
00039 int json_to_corr(JO array, struct correspondence*corr, int n) {
00041         int i;
00042         for(i=0;i<n;i++) {
00043                 JO element = json_object_array_get_idx(array, i);
00044                 if(element == NULL) {
00045                         corr[i].valid = 0;
00046                         corr[i].j1 = -1;
00047                         corr[i].j2 = -1;
00048                 } else {
00049                         corr[i].valid = 1;
00050                         jo_read_int(element, "j1", &(corr[i].j1));
00051                         jo_read_int(element, "j2", &(corr[i].j2));
00052                         int type;
00053                         jo_read_int(element, "type", &(type));
00054                         corr[i].type = type;
00055                 }
00056         }
00057         return 1;
00058 }
00059 
00060 JO corr_to_json(struct correspondence*corr, int n) {
00061         JO jo = jo_new_array();
00062         int i;
00063         for(i=0;i<n;i++) {
00064                 if(corr[i].valid) {
00065                         JO c = jo_new();
00066                         jo_add_int(c, "j1", corr[i].j1);
00067                         jo_add_int(c, "j2", corr[i].j2);
00068                         jo_add_int(c, "type", (int) corr[i].type);
00069                         jo_array_add(jo, c);
00070                 } else {
00071                         jo_array_add(jo, jo_new_null());
00072                 }
00073         }
00074         return jo;
00075 }
00076 
00077 
00078 JO vector_to_json(gsl_vector*vec) {
00079         JO jo = json_object_new_array();
00080         size_t j;
00081         for(j=0;j<vec->size;j++) {
00082                 double v = gsl_vector_get(vec,j);
00083                 jo_array_add(jo, jo_double_or_null(v));
00084         }
00085         return jo;
00086 }
00087 
00088 
00089 void jo_add_timestamp(JO jo, const char*name, struct timeval*tv) {
00090         int array[2] = {tv->tv_sec, tv->tv_usec};
00091         jo_add_int_array(jo, name, array, 2);
00092 }
00093 
00094 JO result_to_json(struct sm_params*p, struct sm_result *r) {
00095         JO jo = json_object_new_object();
00096         jo_add_int(jo, "valid",  r->valid);
00097         
00098         if(r->valid) {
00099                 jo_add_double_array(jo, "x",  r->x, 3);
00100         
00101                 if(p->do_compute_covariance) {
00102                         jo_add(jo, "cov_x",   matrix_to_json(r->cov_x_m ) );
00103                         jo_add(jo, "dx_dy1",  matrix_to_json(r->dx_dy1_m) );
00104                         jo_add(jo, "dx_dy2",  matrix_to_json(r->dx_dy2_m) );
00105                 }
00106         }
00107                 jo_add_int(jo, "iterations", r->iterations);
00108                 jo_add_int(jo, "nvalid", r->nvalid);
00109                 jo_add_double(jo, "error", r->error);
00110                 
00111                 jo_add_timestamp(jo, "laser_ref_timestamp", &(p->laser_ref->tv));
00112                 jo_add_timestamp(jo, "laser_sens_timestamp",  &(p->laser_sens->tv));
00113         
00114         return jo;
00115 }
00116 
00117 int is_all_nan(const double *v, int n ) {
00118         int i; for(i=0;i<n;i++) if(v[i]==v[i]) return 0;
00119         return 1;
00120 }
00121 
00123 void jo_add_double_array_if_not_nan(JO jo, const char*name, const double *v, int n) {
00124         if(is_all_nan(v,n)) return;
00125         jo_add_double_array(jo, name, v, n);
00126 }
00127 
00129 int all_is(int *a, int n, int v) {
00130         int i; for(i=0;i<n;i++) if(a[i]!=v) return 0;
00131         return 1;
00132 }
00133 
00134 JO ld_to_json(LDP ld) {
00135         JO jo = json_object_new_object();
00136         int n = ld->nrays;
00137 
00138         jo_add_int(jo, "nrays",    ld->nrays);
00139         jo_add_double(jo, "min_theta", ld->min_theta);
00140         jo_add_double(jo, "max_theta", ld->max_theta);
00141 
00142         jo_add_double_array(jo, "odometry",   ld->odometry,  3);
00143         jo_add_double_array(jo, "estimate",   ld->estimate,  3);
00144         jo_add_double_array(jo, "true_pose",  ld->true_pose, 3);
00145 
00146         jo_add_double_array(jo, "theta",     ld->theta, n);
00147         jo_add_double_array(jo, "readings",   ld->readings, n);
00148         jo_add_double_array_if_not_nan(jo, "readings_sigma", ld->readings_sigma, n);
00149 
00150         jo_add_int_array(jo, "valid",   ld->valid, n);
00151 
00152         if(!all_is(ld->cluster, n, -1))
00153                 jo_add_int_array(jo, "cluster",  ld->cluster, n);
00154 
00155         jo_add_double_array_if_not_nan(jo, "alpha",        ld->alpha, n);
00156         jo_add_double_array_if_not_nan(jo, "cov_alpha",    ld->cov_alpha, n);
00157 
00158         if(!all_is(ld->alpha_valid, n, 0))
00159                 jo_add_int_array(jo, "alpha_valid", ld->alpha_valid, n);
00160         
00161         jo_add_double_array_if_not_nan(jo, "true_alpha",      ld->true_alpha, n);
00162         
00163         int timeval[2] = {ld->tv.tv_sec, ld->tv.tv_usec};
00164         jo_add_int_array(jo, "timestamp", timeval, 2);
00165         
00166         return jo;
00167 }
00168 
00169 /* TODO: add some checks */
00170 LDP json_to_ld(JO jo) {
00171         int n;
00172         if(!jo_read_int(jo, "nrays", &n)) {
00173                 sm_error("Could not read nrays.\n");
00174                 return 0;
00175         }
00176         
00177         LDP ld = ld_alloc_new(n);
00178         
00179         jo_read_double(jo, "min_theta", &ld->min_theta);
00180         jo_read_double(jo, "max_theta", &ld->max_theta);        
00181         /* XXX add more checks */
00182         jo_read_double_array(jo, "theta", ld->theta, n, NAN);   
00183         jo_read_double_array(jo, "readings", ld->readings, n, NAN);     
00184 
00185         if(jo_has_field(jo,"readings_sigma") && !jo_read_double_array(jo, "readings_sigma", ld->readings_sigma, n, NAN))
00186                 { sm_error("Error while reading field 'readings_sigma'.\n"); return 0; }
00187 
00188         jo_read_int_array(jo, "valid",     ld->valid, n, 0);
00189         jo_read_int_array(jo, "cluster",   ld->cluster, n, -1);
00190 
00191         if(jo_has_field(jo,"alpha") && !jo_read_double_array(jo, "alpha",ld->alpha, n, NAN)) 
00192                 { sm_error("Error while reading field alpha.\n"); return 0; }
00193                 
00194         if(jo_has_field(jo,"cov_alpha") && !jo_read_double_array(jo, "cov_alpha", ld->cov_alpha, n, NAN))
00195                 { sm_error("Error while reading field cov_alpha.\n"); return 0; }
00196         
00197         if(jo_has_field(jo,"alpha_valid") && !jo_read_int_array(jo, "alpha_valid",   ld->alpha_valid, n, 0)) 
00198                 { sm_error("Error while reading field alpha_valid.\n"); return 0; }
00199 
00200         if(jo_has_field(jo,"true_alpha") && !jo_read_double_array(jo, "true_alpha", ld->true_alpha, n, NAN))
00201                 { sm_error("Error while reading field true_alpha.\n"); return 0; }
00202                 
00203         
00204         jo_read_double_array(jo, "odometry", ld->odometry, 3, NAN);
00205         jo_read_double_array(jo, "estimate", ld->estimate, 3, NAN);     
00206         jo_read_double_array(jo, "true_pose", ld->true_pose, 3, NAN);   
00207         
00208         int array[2] = {-1,-1};
00209         jo_read_int_array(jo, "timestamp", array, 2,-1);
00210         ld->tv.tv_sec = array[0];
00211         ld->tv.tv_usec = array[1];
00212         
00213         return ld;
00214 }
00215 
00216 LDP ld_from_json_string(const char*s) {
00217         JO jo = json_parse(s);
00218         if(!jo) {
00219                 sm_error("Invalid JSON found.\n");
00220                 return 0;
00221         }
00222         LDP ld = json_to_ld(jo);
00223         if(!ld) {
00224                 sm_error("Could not read laser_data:\n\n%s\n", json_object_to_json_string(jo));
00225                 return 0;
00226         }
00227         jo_free(jo);
00228         return ld;
00229 }
00230 
00231 LDP ld_from_json_stream(FILE*file) {
00232         JO jo; /* the monkey */
00233         LDP ld;
00234         
00235         jo = json_read_stream(file);
00236         if(!jo) {
00237                 if(!feof(file)) {
00238                         fprintf(stderr, " (!)\n");
00239                         sm_error("Invalid JSON found.\n");
00240                 }
00241                 fprintf(stderr, " EOF\n");
00242                 
00243                 return 0;
00244         }
00245 
00246         ld = json_to_ld(jo);
00247         if(!ld) {
00248                 sm_error("Could not read laser_data:\n\n%s\n", json_object_to_json_string(jo));
00249                 return 0;
00250         }
00251         jo_free(jo);
00252         
00253         fprintf(stderr, "l");
00254         
00255         return ld;
00256 }
00257 
00258 void ld_write_as_json(LDP ld, FILE * stream) {
00259         if(!ld_valid_fields(ld)) {
00260                 sm_error("Writing bad data to the stream.\n");
00261         }
00262 
00263         JO jo = ld_to_json(ld);
00264         fputs(json_object_to_json_string(jo), stream);
00265         fputs("\n", stream);
00266         jo_free(jo);
00267 }
00268 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42