laser_data_carmen.c
Go to the documentation of this file.
00001 #include <string.h>
00002 #include <math.h>
00003 #include <ctype.h>
00004 
00005 #include "csm_all.h"
00006 
00007 const char * carmen_prefix = "FLASER ";
00008 
00010 int read_next_double(const char*line, size_t*cur, double*d);
00011 
00013 int read_next_integer(const char*line, size_t*cur, int*d);
00014 
00016 int read_next_string(const char*line, size_t*cur, char*buf, size_t buf_len);
00017 
00018 
00020 int read_next_double(const char*line, size_t*cur, double*d) {
00021         int inc;
00022         int ret = sscanf(line+*cur, " %lf %n", d, &inc);
00023         if(1 != ret) {
00024                 sm_error("Could not read double at %p + %d '%s'. ret: %d.\n", line, *cur, line+*cur, ret);
00025                 return -1;
00026         }
00027         *cur += inc;
00028         return 0;
00029 }
00030 
00032 int read_next_integer(const char*line, size_t*cur, int*d) {
00033         int inc;
00034         if(1 != sscanf(line+*cur, " %d %n", d, &inc)) {
00035         /*      sm_error("Could not read integer.\n");*/
00036                 return -1;
00037         }
00038         *cur += inc;
00039         return 0;
00040 }
00041 
00043 int read_next_string(const char*line, size_t*cur, char*buf, size_t buf_len) {
00044         int from = *cur; while(isspace(line[from])) from++;
00045         size_t len = 0; while(!isspace(line[from+len])) len++;
00046         if(len > buf_len ) len = buf_len;
00047         strncpy(buf, line+from, len);
00048         *cur += len;
00049         return 0;
00050 }
00051 
00052 LDP ld_from_carmen_string(const char*line) {
00053         
00054         if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
00055                 sm_error("This is not a Carmen line: \n-> %s\n", line);
00056                 return 0;
00057         }
00058         
00059         size_t cur = strlen(carmen_prefix); 
00060 
00061         
00062         int nrays=-1;
00063         if(read_next_integer(line, &cur, &nrays)) {
00064                 sm_error("Could not get number of rays.\n");
00065                 goto error;
00066         }
00067 
00068         LDP ld = ld_alloc_new(nrays);
00069         
00070         
00071         double fov = M_PI;
00072         double min_reading = 0;
00073         double max_reading = 80;
00074         
00075         if(nrays == 769) {
00076                 min_reading = 0.001;
00077                 max_reading = 4;
00078                 fov = deg2rad(270.0);
00079 
00080                 static int print = 0;
00081                 if(!print) { print = 1;
00082                         sm_info("Assuming that 769 rays is an Hokuyo "
00083                          "with fov = %f deg, min_reading = %f m, max_reading = %fm\n",
00084                                 rad2deg(fov), min_reading, max_reading);
00085                 }
00086         }
00087         
00088         ld->min_theta = -fov/2;
00089         ld->max_theta = +fov/2;
00090         
00091         int on_error = 0;
00092         int i;
00093         for(i=0;i<nrays;i++) {
00094                 double reading;
00095                 if(read_next_double(line,&cur,&reading)) {
00096                         sm_error("Could not read ray #%d / %d, \n", i, nrays); 
00097                         on_error = 1;
00098                         break;
00099                 }
00100                         
00101                 ld->valid[i] = (reading > min_reading) && (reading < max_reading);
00102                 ld->readings[i] = ld->valid[i] ? reading : NAN;
00103                 ld->theta[i] = ld->min_theta + i * 
00104                   (ld->max_theta-ld->min_theta) / (ld->nrays-1);
00105                 
00106                 /* bad hokuyo!! */
00107                 if(nrays == 769) {
00108                         if(i>725 || i<44) { 
00109                                 ld->valid[i] = 0; 
00110                                 ld->readings[i] = NAN;
00111                         }
00112                 }
00113                 
00114         }
00115         
00116         if(on_error) goto error;
00117         
00118         if(read_next_double(line,&cur,ld->estimate+0)) goto error;
00119         if(read_next_double(line,&cur,ld->estimate+1)) goto error;
00120         if(read_next_double(line,&cur,ld->estimate+2)) goto error;
00121         if(read_next_double(line,&cur,ld->odometry+0)) goto error;
00122         if(read_next_double(line,&cur,ld->odometry+1)) goto error;
00123         if(read_next_double(line,&cur,ld->odometry+2)) goto error;
00124 
00125         /* Following: ipc_timestamp hostname timestamp */
00126         /* Two options:
00127                 double string double: 
00128                         the first is timestamp in seconds, the second is discarded
00129                 int string int:
00130                         the first is sec, the second is usec 
00131         */
00132         static int warn_format = 1;
00133 
00134         int inc; int sec=-1, usec=-1;
00135         int res = sscanf(line + cur, "%d %s %d%n", &sec, ld->hostname, &usec,  &inc);
00136         if(3 == res) {
00137                 ld->tv.tv_sec = sec;
00138                 ld->tv.tv_usec = usec;
00139                 if(warn_format)
00140                         sm_info("Reading timestamp as 'sec hostname usec'.\n");
00141         } else {
00142                 double v1=-1, v2=-1;
00143                 res = sscanf(line + cur, "%lf %s %lf%n", &v1, ld->hostname, &v2,  &inc);
00144                 if(3 == res) {
00145                         ld->tv.tv_sec = (int) floor(v1);
00146                         ld->tv.tv_usec = floor( (v1 - floor(v1)) * 1e6 );
00147                         
00148                         if(warn_format)
00149                                 sm_info("Reading timestamp as doubles (discarding second one).\n");
00150                         
00151                 } else {
00152                         ld->tv.tv_sec = 0;
00153                         ld->tv.tv_usec = 0;
00154                         if(warn_format)
00155                                 sm_info("I could not read timestamp+hostname; ignoring (I will warn only once for this).\n");
00156                 }
00157         }
00158 
00159         warn_format = 0;
00160 
00161         fprintf(stderr, "l");
00162         return ld;
00163         
00164         error:
00165                 printf("Malformed line: '%s'\nat cur = %d\n\t-> '%s'\n", line,(int)cur,line+cur);
00166                 return 0;
00167 }
00168 
00170 int ld_read_next_laser_carmen(FILE*file, LDP*ld) {
00171         *ld = 0;
00172         #define MAX_LINE_LENGTH 10000
00173    char line[MAX_LINE_LENGTH];
00174 
00175         while(fgets(line, MAX_LINE_LENGTH-1, file)) {
00176                 if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
00177                         sm_debug("Skipping line: \n-> %s\n", line);
00178                         continue;
00179                 }
00180                 
00181                 *ld = ld_from_carmen_string(line);
00182                 if(!*ld) {
00183                         printf("Malformed line? \n-> '%s'", line);
00184                         return 0;
00185                 } else {
00186                         return 1;
00187                 }
00188         }
00189         return 1;
00190 }
00191 
00193 void ld_write_as_carmen(LDP ld, FILE * stream) {
00194         int i;
00195         double timestamp;
00196         if(!ld_valid_fields(ld)) {
00197                 sm_error("Writing bad data to the stream.\n");
00198         }
00199         fprintf(stream, "FLASER %d ", ld->nrays);
00200         for(i=0; i<ld->nrays; i++){
00201                 fprintf(stream, "%g ", ld->readings[i]);
00202         }
00203         fprintf(stream, "%g %g %g ", ld->estimate[0], ld->estimate[1], ld->estimate[2]);
00204         fprintf(stream, "%g %g %g ", ld->odometry[0], ld->odometry[1], ld->odometry[2]);
00205         
00206         timestamp = ld->tv.tv_sec + ((double)ld->tv.tv_sec)/1e6;
00207         
00208         fprintf(stream, "%g %s %g", timestamp, ld->hostname, timestamp);
00209         
00210         fputs("\n", stream);
00211 }
00212 
00213 void ld_write_format(LDP ld, FILE*f, const char * out_format) {
00214         if(!strncmp(out_format, "carmen", 6))
00215                 ld_write_as_carmen(ld, f);
00216         else
00217                 ld_write_as_json(ld, f);
00218         /* XXX: check validity of format string */
00219 }
00220 
00221 
00222 
00223 


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