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
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
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
00126
00127
00128
00129
00130
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
00219 }
00220
00221
00222
00223