maybe bug?

rsm/rsm_laserdata_carmen.rb

@@ -13,7 +13,7 @@ class LaserData
 		
 		ld = LaserData.new(nrays)
 				
-		ld.min_reading   = 0.1;
+		ld.min_reading   = 0.001;
 		ld.max_reading   = 49;
 		ld.min_theta     = -PI/2;
 		ld.max_theta     =  PI/2;
@@ -29,7 +29,7 @@ class LaserData
 				raise "Bad value (#{p.reading}) for ray#{a} in '#{l}'"
 			end
 			
-			ld.theta[i] = -PI/2 + i*PI/(ld.nrays-1)
+			ld.theta[i] = ld.min_theta + i * (ld.max_theta) / (ld.nrays-1)
 			ld.valid[i] = (reading < ld.max_reading) && (reading > ld.min_reading)
 			ld.readings[i] = ld.valid[i] ? reading : GSL::NAN
 

sm/csm/laser_data_carmen.c

 #include <string.h>
 #include <math.h>
+#include <ctype.h>
 
 #include "csm_all.h"
 
+/** Returns 0 on success */
+int read_next_double(const char*line, size_t*cur, double*d);
+
+/** Returns 0 on success */
+int read_next_integer(const char*line, size_t*cur, int*d);
+
+/** Always returns 0 */
+int read_next_string(const char*line, size_t*cur, char*buf, size_t buf_len);
+
 const char * carmen_prefix = "FLASER ";
 
 /** Returns 0 on success */
-int read_next_double(const char*line, int*cur, double*d) {
+int read_next_double(const char*line, size_t*cur, double*d) {
 	int inc;
 	if(1 != sscanf(line+*cur, " %lf %n", d, &inc)) {
-		printf("Could not read double.\n");
+		sm_error("Could not read double.\n");
+		return -1;
+	}
+	*cur += inc;
+	return 0;
+}
+
+/** Returns 0 on success */
+int read_next_integer(const char*line, size_t*cur, int*d) {
+	int inc;
+	if(1 != sscanf(line+*cur, " %d %n", d, &inc)) {
+		sm_error("Could not read integer.\n");
 		return -1;
 	}
 	*cur += inc;
 	return 0;
 }
 
+/** Always returns 0 */
+int read_next_string(const char*line, size_t*cur, char*buf, size_t buf_len) {
+	int from = *cur; while(isspace(line[from])) from++;
+	size_t len = 0; while(!isspace(line[from+len])) len++;
+	if(len > buf_len ) len = buf_len;
+	strncpy(buf, line+from, len);
+	*cur += len;
+	return 0;
+}
+
 /** Read next FLASER line in file (initializes ld). Returns !=0 if error or EOF. */
 int ld_read_next_laser_carmen(FILE*file, LDP ld) {
 	#define MAX_LINE_LENGTH 10000
@@ -28,7 +59,7 @@ int ld_read_next_laser_carmen(FILE*file, LDP ld) {
 			continue;
 		}
 		
-		int cur = strlen(carmen_prefix); int inc;
+		size_t cur = strlen(carmen_prefix); int inc;
 		
 		int nrays;
 		if(1 != sscanf(line+cur, "%d %n", &nrays, &inc)) {
@@ -39,8 +70,25 @@ int ld_read_next_laser_carmen(FILE*file, LDP ld) {
 			
 		ld_alloc(ld, nrays);	
 		
-		ld->min_theta = -M_PI/2;
-		ld->max_theta = +M_PI/2;
+		double fov = M_PI;
+		double min_reading = 0;
+		double max_reading = 80;
+		
+		if(nrays == 769) {
+			min_reading = 0.03;
+			max_reading = 8;
+			fov = deg2rad(270.0);
+
+			static int print = 0;
+			if(!print) { print = 1;
+				sm_debug("Assuming that 769 rays is an Hokuyo "
+				 "with fov = %f deg, min_reading = %f m, max_reading = %fm\n",
+					rad2deg(fov), min_reading, max_reading);
+			}
+		}
+		
+		ld->min_theta = -fov/2;
+		ld->max_theta = +fov/2;
 		
 		int i;
 		for(i=0;i<nrays;i++) {
@@ -50,9 +98,10 @@ int ld_read_next_laser_carmen(FILE*file, LDP ld) {
 				goto error;
 			}
 				
-			ld->valid[i] = reading>0 && reading<80;
+			ld->valid[i] = (reading > min_reading) && (reading < max_reading);
 			ld->readings[i] = ld->valid[i] ? reading : NAN;
-			ld->theta[i] = ld->min_theta+ i * (ld->max_theta-ld->min_theta) / (ld->nrays-1);
+			ld->theta[i] = ld->min_theta + i * 
+			  (ld->max_theta-ld->min_theta) / (ld->nrays-1);
 		}
 		
 		if(read_next_double(line,&cur,ld->estimate+0)) goto error;
@@ -63,12 +112,20 @@ int ld_read_next_laser_carmen(FILE*file, LDP ld) {
 		if(read_next_double(line,&cur,ld->odometry+2)) goto error;
 
 		/* Following: ipc_timestamp hostname timestamp */
+		char buf[30];
+		int sec, usec;
+		if(read_next_integer(line, &cur, &sec )) goto error;
+		if(read_next_string(line, &cur, buf, 29)) goto error;
+		if(read_next_integer(line, &cur, &usec )) goto error;
+		
+		ld->tv.tv_sec = sec;
+		ld->tv.tv_usec = usec;
 		
 		fprintf(stderr, "l");
 		return 0;
 		
 		error:
-			printf("Malformed line? \n-> %s\nat cur = %d\n\t-> %s\n", line,cur,line+cur);
+			printf("Malformed line? \n-> %s\nat cur = %d\n\t-> %s\n", line,(int)cur,line+cur);
 			return -1;
 	}
 	return -2;

sm/csm/laser_data_json.c

@@ -139,7 +139,9 @@ JO ld_to_json(LDP ld) {
 	
 	jo_add_double_array_if_not_nan(jo, "true_alpha",      ld->true_alpha, n);
 	
-
+	int timeval[2] = {ld->tv.tv_sec, ld->tv.tv_usec};
+	jo_add_int_array(jo, "timestamp", timeval, 2);
+	
 	return jo;
 /*	int *up_bigger, *up_smaller, *down_bigger, *down_smaller;
 

sm/csm/logging.c

@@ -6,7 +6,7 @@
 
 const char * sm_program_name = 0;
 
-/*#define CSM_DEBUG */
+#define CSM_DEBUG 
 
 void sm_error(const char *msg, ...)
 {

sm/lib/options/options.c

@@ -148,15 +148,20 @@ int options_parse_stream(struct option*ops, const char*pwd, FILE*file) {
 }
 
 int options_parse_file(struct option*ops, const char*pwd, const char*filename) {
+	fprintf(stderr, "Filename lne=%d s='%s'\n", (int)strlen(filename), filename);
 	char concat[PATH_MAX*2+1];
 	strcpy(concat, pwd);
 	strcat(concat, "/");
 	strcat(concat, filename);
+
+	fprintf(stderr, "concat ='%s'\n", concat);
+
 	char *resolved;
 	if(! (resolved = realpath(concat, NULL))) {
 		fprintf(stderr, "Could not resolve '%s' ('%s').\n", concat, resolved); 
 		return 0;
 	}
+	fprintf(stderr, "resolved ='%s'\n", resolved);
 
 	const char * newdir = dirname(resolved);
 	if(!newdir) {
@@ -244,7 +249,7 @@ int options_set(struct option*o, const char*value) {
 
 const char*options_value_as_string(struct option*o);
 
-void display_table(FILE*f, const char**table, int rows, int columns, int padding) {
+void display_table(FILE*f,  char**table, int rows, int columns, int padding) {
 /*	int *col_size = malloc(sizeof(int)*columns);*/
 	int col_size[columns];
 	
@@ -272,7 +277,7 @@ void display_table(FILE*f, const char**table, int rows, int columns, int padding
 void options_dump(struct option * options, FILE*f, int write_desc) {
 	int n; for (n=0;options_valid(options+n);n++);
 
-	const char**table = malloc(sizeof(char*)*n*3);
+	char**table = malloc(sizeof(char*)*n*3);
 
 	int i;
 	for (i=0;i<n;i++) {