static void carmen_map_read_gridmap(carmen_FILE *fin, carmen_FILE *fout)
{
int size_x, size_y;
float resolution;
float *complete_map = NULL;
carmen_map_t map;
int chunk_type, chunk_size;
char chunk_description[12];
if(carmen_map_advance_to_chunk(fin, CARMEN_MAP_GRIDMAP_CHUNK) < 0) {
fprintf(stderr, "Error: Could not find a gridmap chunk.\n");
fprintf(stderr, " This file is probably not a map file.\n");
carmen_fclose(fin);
}
chunk_type = carmen_fgetc(fin);
carmen_fread(&chunk_size, sizeof(int), 1, fin);
carmen_fread(chunk_description, 10, 1, fin);
chunk_description[10] = '\0';
if(CARMEN_MAP_CHUNK_IS_NAMED(chunk_type)) {
if(read_string(NULL, -1, fin) < 0) {
carmen_warn("Error: Unexpected EOF.\n");
carmen_fclose(fin);
}
}
carmen_fread(&size_x, sizeof(int), 1, fin);
carmen_fread(&size_y, sizeof(int), 1, fin);
carmen_fread(&resolution, sizeof(float), 1, fin);
printf("%d %d %lf\n", size_x, size_y, resolution);
carmen_grid_mapping_create_new_map(&map, size_x, size_y, resolution);
complete_map = (float *)calloc(map.config.x_size * map.config.y_size, sizeof(float));
carmen_fread(complete_map, sizeof(float) * size_x * size_y, 1, fin);
for (int i = 0; i < map.config.x_size * map.config.y_size; i++)
{
map.complete_map[i] = complete_map[i];
}
carmen_map_write_all(fout, map.map,
map.config.x_size,
map.config.y_size,
map.config.resolution,
(char *)"", (char *)"", (char *)"", (char *)"Generated by LCAD-UFES",
NULL, 0, NULL, 0, NULL, 0);
free(complete_map);
free(map.complete_map);
free(map.map);
}
int carmen_logfile_read_line(carmen_logfile_index_p index, carmen_FILE *infile,
int message_num, int max_line_length, char *line)
{
size_t nread;
/* are we moving sequentially through the logfile? If not, fseek */
if(message_num != index->current_position) {
index->current_position = message_num;
carmen_fseek(infile, index->offset[index->current_position], SEEK_SET);
}
/* check maximum line length */
if(index->offset[index->current_position + 1] -
index->offset[index->current_position] >= max_line_length)
carmen_die("Error: exceed maximum line length.\n");
/* read the line of the logfile */
nread = carmen_fread(line, 1, index->offset[index->current_position + 1] -
index->offset[index->current_position], infile);
line[nread] = '\0';
index->current_position++;
return nread;
}
off_t carmen_logfile_uncompressed_length(carmen_FILE *infile)
{
unsigned char buffer[10000];
long int log_bytes = 0;
int nread;
struct stat stat_buf;
if(!infile->compressed) {
/* compute total length of logfile */
carmen_fseek(infile, 0L, SEEK_SET);
log_bytes = 0;
do {
nread = carmen_fread(buffer, 1, 10000, infile);
log_bytes += nread;
} while(nread > 0);
carmen_fseek(infile, 0L, SEEK_SET);
return log_bytes;
}
else {
/* report compressed size for compressed files */
fstat(fileno(infile->fp), &stat_buf);
return stat_buf.st_size;
}
}
/**
* Builds the index structure used for parsing a carmen log file.
**/
carmen_logfile_index_p carmen_logfile_index_messages(carmen_FILE *infile)
{
carmen_logfile_index_p index;
int i, found_linebreak = 1, nread, max_messages;
off_t file_length = 0, file_position = 0, total_bytes, read_count = 0;
unsigned char buffer[10000];
// KMW: set numeric locale to C, so we can correctly read decimal numbers.
// This fixes problems if your locale is set to, lets say, de_DE.utf8
setlocale (LC_NUMERIC,"C");
/* allocate and initialize an index */
index = (carmen_logfile_index_p)calloc(1, sizeof(carmen_logfile_index_t));
carmen_test_alloc(index);
/* compute the total length of the uncompressed logfile. */
fprintf(stderr, "\n\rIndexing messages (0%%) ");
file_length = carmen_logfile_uncompressed_length(infile);
/* mark the start of all messages */
index->num_messages = 0;
max_messages = 10000;
index->offset = (off_t*)calloc(max_messages, sizeof(off_t));
carmen_test_alloc(index->offset);
carmen_fseek(infile, 0L, SEEK_SET);
total_bytes = 0;
do {
nread = carmen_fread(buffer, 1, 10000, infile);
read_count++;
if(read_count % 1000 == 0) {
if(!infile->compressed)
file_position = total_bytes + nread;
else
file_position = lseek(fileno(infile->fp), 0, SEEK_CUR);
fprintf(stderr, "\rIndexing messages (%.0f%%) ",
((float)file_position) / file_length * 100.0);
}
if(nread > 0) {
for(i = 0; i < nread; i++) {
if(found_linebreak && buffer[i] != '\r') {
found_linebreak = 0;
if(index->num_messages == max_messages) {
max_messages += 10000;
index->offset = (off_t*)realloc(index->offset, max_messages *
sizeof(off_t));
carmen_test_alloc(index->offset);
}
index->offset[index->num_messages] = total_bytes + i;
index->num_messages++;
}
if(buffer[i] == '\n')
found_linebreak = 1;
}
total_bytes += nread;
}
} while(nread > 0);
// set file size as last offset
// offset array now contains one element more than messages
// required by carmen_logfile_read_line to read the last line
if(index->num_messages == max_messages) {
max_messages += 1;
index->offset = (off_t*)realloc(index->offset, max_messages * sizeof(off_t));
carmen_test_alloc(index->offset);
}
index->offset[index->num_messages] = total_bytes;
fprintf(stderr, "\rIndexing messages (100%%) - %d messages found. \n",
index->num_messages);
carmen_fseek(infile, 0L, SEEK_SET);
index->current_position = 0;
return index;
}
glv_object_p glv_object_read(char *filename) {
carmen_FILE *fp;
glv_color_t current_color;
char buffer[10000];
long int nread, log_bytes = 0;
int buffer_pos, buffer_length, offset = 0, n;
glv_object_p obj;
float *fmessage;
int done_reading = 0;
int i, line_count = 0;
float min_x, max_x, min_y, max_y, min_z, max_z;
if(strncmp(filename + strlen(filename) - 4, ".glv", 4) &&
strncmp(filename + strlen(filename) - 7, ".glv.gz", 7) &&
strncmp(filename + strlen(filename) - 5, ".pmap", 5) &&
strncmp(filename + strlen(filename) - 8, ".pmap.gz", 8))
carmen_die("Error: file name must end in .glv, .glv.gz, .pmap, or .pmap.gz\n");
fp = carmen_fopen(filename, "r");
/* compute total number of bytes in logfile */
do {
nread = carmen_fread(buffer, 1, 10000, fp);
log_bytes += nread;
} while(nread > 0);
carmen_fseek(fp, 0L, SEEK_SET);
current_color.r = 255;
current_color.g = 255;
current_color.b = 255;
obj = glv_object_init();
buffer_pos = 0;
buffer_length = carmen_fread(buffer, 1, 10000, fp);
while(!done_reading || buffer_length > buffer_pos) {
line_count++;
if(line_count % 100000 == 0)
fprintf(stderr, "\rReading glv file... (%.0f%%) ",
(offset + buffer_pos) / (float)log_bytes * 100.0);
if(buffer_length - buffer_pos < 50 && !done_reading) {
memmove(buffer, buffer + buffer_pos, buffer_length - buffer_pos);
buffer_length -= buffer_pos;
offset += buffer_pos;
buffer_pos = 0;
n = carmen_fread(buffer + buffer_length,
1, 10000 - buffer_length - 1, fp);
if(n == 0)
done_reading = 1;
else
buffer_length += n;
}
else {
if(buffer[buffer_pos] == MESSAGE_ID_COLOR) {
current_color.r = (unsigned char)buffer[buffer_pos + 1];
current_color.g = (unsigned char)buffer[buffer_pos + 2];
current_color.b = (unsigned char)buffer[buffer_pos + 3];
buffer_pos += 4;
}
else if(buffer[buffer_pos] == MESSAGE_ID_POINT) {
if(obj->num_points == obj->max_points) {
obj->max_points += 100000;
obj->point = (glv_point_p)realloc(obj->point, obj->max_points *
sizeof(glv_point_t));
carmen_test_alloc(obj->point);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->point[obj->num_points].x = fmessage[0];
obj->point[obj->num_points].y = fmessage[1];
obj->point[obj->num_points].z = fmessage[2];
obj->point[obj->num_points].c = current_color;
obj->num_points++;
buffer_pos += 13;
}
else if(buffer[buffer_pos] == MESSAGE_ID_LINE) {
if(obj->num_lines == obj->max_lines) {
obj->max_lines += 100000;
obj->line = (glv_line_p)realloc(obj->line, obj->max_lines *
sizeof(glv_line_t));
carmen_test_alloc(obj->line);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->line[obj->num_lines].p1.x = fmessage[0];
obj->line[obj->num_lines].p1.y = fmessage[1];
obj->line[obj->num_lines].p1.z = fmessage[2];
obj->line[obj->num_lines].p2.x = fmessage[3];
obj->line[obj->num_lines].p2.y = fmessage[4];
obj->line[obj->num_lines].p2.z = fmessage[5];
obj->line[obj->num_lines].c = current_color;
obj->num_lines++;
buffer_pos += 25;
}
else if(buffer[buffer_pos] == MESSAGE_ID_FACE) {
if(obj->num_faces == obj->max_faces) {
obj->max_faces += 100000;
obj->face = (glv_face_p)realloc(obj->face, obj->max_faces *
sizeof(glv_face_t));
carmen_test_alloc(obj->face);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->face[obj->num_faces].p1.x = fmessage[0];
obj->face[obj->num_faces].p1.y = fmessage[1];
obj->face[obj->num_faces].p1.z = fmessage[2];
obj->face[obj->num_faces].p2.x = fmessage[3];
obj->face[obj->num_faces].p2.y = fmessage[4];
obj->face[obj->num_faces].p2.z = fmessage[5];
obj->face[obj->num_faces].p3.x = fmessage[6];
obj->face[obj->num_faces].p3.y = fmessage[7];
obj->face[obj->num_faces].p3.z = fmessage[8];
obj->face[obj->num_faces].c = current_color;
compute_normal(&obj->face[obj->num_faces]);
obj->num_faces++;
buffer_pos += 37;
}
}
}
min_x = 1e10;
max_x = -1e10;
min_y = 1e10;
max_y = -1e10;
min_z = 1e10;
max_z = -1e10;
for(i = 0; i < obj->num_points; i++)
adjust_extrema(obj->point[i], &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
for(i = 0; i < obj->num_lines; i++) {
adjust_extrema(obj->line[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->line[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
for(i = 0; i < obj->num_faces; i++) {
adjust_extrema(obj->face[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p3, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
obj->centroid.x = (min_x + max_x) / 2;
obj->centroid.y = (min_y + max_y) / 2;
obj->centroid.z = (min_z + max_z) / 2;
obj->min.x = min_x;
obj->min.y = min_y;
obj->min.z = min_z;
obj->max.x = max_x;
obj->max.y = max_y;
obj->max.z = max_z;
carmen_fclose(fp);
fprintf(stderr, "\rReading glv file... (100%%) \n");
fprintf(stderr, "%d POINTS - %d LINES - %d FACES\n", obj->num_points,
obj->num_lines, obj->num_faces);
return obj;
}
void read_nsick_logfile(char *filename, logdata_p logdata) {
int buffer_pos, buffer_length, offset = 0;
int linecount = 0, mark, n, i;
long int nread, log_bytes = 0;
carmen_FILE *log_fp = NULL;
char *current_pos;
char buffer[10000];
int laser_num_readings;
double laser_start_angle, laser_fov;
/* initialize logdata structure */
logdata->num_laser = 0;
logdata->max_laser = 1000;
logdata->laser = (laser_scan_p)calloc(logdata->max_laser,
sizeof(laser_scan_t));
carmen_test_alloc(logdata->laser);
logdata->num_pos = 0;
logdata->max_pos = 1000;
logdata->pos = (laser_pos_p)calloc(logdata->max_pos, sizeof(laser_pos_t));
carmen_test_alloc(logdata->pos);
/* compute total number of bytes in logfile */
log_fp = carmen_fopen(filename, "r");
if (log_fp == NULL)
carmen_die("Error: could not open file %s for reading.\n", filename);
do {
nread = carmen_fread(buffer, 1, 10000, log_fp);
log_bytes += nread;
}
while (nread > 0);
carmen_fseek(log_fp, 0L, SEEK_SET);
/* read the logfile */
buffer_pos = 0;
buffer_length = carmen_fread(buffer, 1, 10000, log_fp);
while (buffer_length > 0) {
mark = buffer_pos;
while ((mark < buffer_length) && (buffer[mark] != '\n'))
++mark;
if (mark == buffer_length) {
memmove(buffer, buffer+buffer_pos, buffer_length-buffer_pos);
buffer_length -= buffer_pos;
offset += buffer_pos;
buffer_pos = 0;
n = carmen_fread(buffer+buffer_length, 1, 10000-buffer_length-1, log_fp);
buffer_length += n;
}
else {
++linecount;
if (linecount % 100 == 0)
fprintf(stderr, "\rReading log file %s... (%.0f%%) ", filename,
(offset+buffer_pos)/(float)log_bytes*100.0);
buffer[mark] = '\0';
if (!strncmp(buffer+buffer_pos, "NSICKLASERPOS", 13)) {
if (logdata->num_pos == logdata->max_pos) {
logdata->max_pos += 1000;
logdata->pos = (laser_pos_p)realloc(logdata->pos,
logdata->max_pos*sizeof(laser_pos_t));
carmen_test_alloc(logdata->pos);
}
current_pos = buffer+buffer_pos;
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].laser_num = atoi(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].x = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].y = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].z = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].yaw = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].pitch = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].roll = atof(current_pos);
current_pos = carmen_next_word(current_pos);
logdata->pos[logdata->num_pos].timestamp = atof(current_pos);
++logdata->num_pos;
}
else if (logdata->num_pos &&
!strncmp(buffer+buffer_pos, "NSICKLASER", 10) &&
buffer[buffer_pos+10] ==
'0'+logdata->pos[logdata->num_pos-1].laser_num) {
if (logdata->num_laser == logdata->max_laser) {
logdata->max_laser += 1000;
logdata->laser = (laser_scan_p)realloc(logdata->laser,
logdata->max_laser*sizeof(laser_scan_t));
carmen_test_alloc(logdata->laser);
}
current_pos = buffer+buffer_pos;
logdata->laser[logdata->num_laser].laser_num =
logdata->pos[logdata->num_pos].laser_num;
current_pos = carmen_next_n_words(current_pos, 2);
laser_start_angle = atof(current_pos);
current_pos = carmen_next_word(current_pos);
laser_fov = atof(current_pos);
current_pos = carmen_next_n_words(current_pos, 5);
laser_num_readings = atoi(current_pos);
logdata->laser[logdata->num_laser].num_readings = laser_num_readings;
logdata->laser[logdata->num_laser].start_angle = laser_start_angle;
logdata->laser[logdata->num_laser].fov = laser_fov;
logdata->laser[logdata->num_laser].range =
(float*)calloc(laser_num_readings, sizeof(float));
carmen_test_alloc(logdata->laser[logdata->num_laser].range);
logdata->laser[logdata->num_laser].endpoint_x =
(float*)calloc(laser_num_readings, sizeof(float));
carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_x);
logdata->laser[logdata->num_laser].endpoint_y =
(float*)calloc(laser_num_readings, sizeof(float));
carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_y);
logdata->laser[logdata->num_laser].endpoint_z =
(float*)calloc(laser_num_readings, sizeof(float));
carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_z);
current_pos = carmen_next_word(current_pos);
for (i = 0; i < laser_num_readings; i++) {
logdata->laser[logdata->num_laser].range[i] = atof(current_pos);
current_pos = carmen_next_word(current_pos);
}
current_pos = carmen_next_word(current_pos);
laser_num_readings = atoi(current_pos);
logdata->laser[logdata->num_laser].timestamp = atof(current_pos);
logdata->num_laser++;
}
buffer_pos = mark+1;
}
}
fprintf(stderr, "\nRead %d LASER - %d POS\n",
logdata->num_laser, logdata->num_pos);
}
void read_firecam_logfile(char *filename, logdata_p logdata) {
int buffer_pos, buffer_length, offset = 0;
int linecount = 0, mark, n;
long int nread, log_bytes = 0;
carmen_FILE *log_fp = NULL;
char *current_pos;
char buffer[10000];
/* initialize logdata structure */
logdata->num_frame = 0;
logdata->max_frame = 1000;
logdata->frame = (firecam_frame_p)calloc(logdata->max_frame,
sizeof(firecam_frame_t));
carmen_test_alloc(logdata->frame);
/* compute total number of bytes in logfile */
log_fp = carmen_fopen(filename, "r");
if (log_fp == NULL)
carmen_die("Error: could not open file %s for reading.\n", filename);
do {
nread = carmen_fread(buffer, 1, 10000, log_fp);
log_bytes += nread;
}
while (nread > 0);
carmen_fseek(log_fp, 0L, SEEK_SET);
/* read the logfile */
buffer_pos = 0;
buffer_length = carmen_fread(buffer, 1, 10000, log_fp);
while (buffer_length > 0) {
mark = buffer_pos;
while ((mark < buffer_length) && (buffer[mark] != '\n'))
++mark;
if (mark == buffer_length) {
memmove(buffer, buffer+buffer_pos, buffer_length-buffer_pos);
buffer_length -= buffer_pos;
offset += buffer_pos;
buffer_pos = 0;
n = carmen_fread(buffer+buffer_length, 1, 10000-buffer_length-1, log_fp);
buffer_length += n;
}
else {
++linecount;
if (linecount % 100 == 0)
fprintf(stderr, "\rReading log file %s... (%.0f%%) ", filename,
(offset+buffer_pos)/(float)log_bytes*100.0);
buffer[mark] = '\0';
if (!strncmp(buffer+buffer_pos, "FIRECAMFRAME", 12)) {
if (logdata->num_frame == logdata->max_frame) {
logdata->max_frame += 1000;
logdata->frame = (firecam_frame_p)realloc(logdata->frame,
logdata->max_frame*sizeof(firecam_frame_t));
carmen_test_alloc(logdata->frame);
}
current_pos = buffer+buffer_pos;
current_pos = carmen_next_word(current_pos);
logdata->frame[logdata->num_frame].cam_id = atoi(current_pos);
copy_filename_string(&logdata->frame[logdata->num_frame].filename,
¤t_pos);
current_pos = carmen_next_word(current_pos);
logdata->frame[logdata->num_frame].timestamp = atof(current_pos);
++logdata->num_frame;
}
buffer_pos = mark+1;
}
}
fprintf(stderr, "\nRead %d FRAME\n", logdata->num_frame);
}