carmen_logfile_index_p load_logindex_file(char *index_file_name)
{
FILE *index_file;
carmen_logfile_index_p logfile_index;
index_file = fopen(index_file_name, "r");
if (index_file == NULL)
carmen_die("Error: could not open file %s for reading.\n", index_file_name);
logfile_index = (carmen_logfile_index_p) malloc(sizeof(carmen_logfile_index_t));
carmen_test_alloc(logfile_index);
fread(logfile_index, sizeof(carmen_logfile_index_t), 1, index_file);
// carmen_logfile_index_messages() (in readlog.c) set file size as last offset
// so, offset array contains one element more than messages
// it is required by carmen_logfile_read_line to read the last line
logfile_index->offset = (off_t *) malloc((logfile_index->num_messages + 1) * sizeof(off_t));
fread(logfile_index->offset, sizeof(off_t), logfile_index->num_messages + 1, index_file);
logfile_index->current_position = 0;
fclose(index_file);
return (logfile_index);
}
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;
}
int
main(int argc, char **argv)
{
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
read_parameters(argc, argv);
if (carmen_navigator_ackerman_initialize_ipc() < 0)
carmen_die("Error: could not connect to IPC Server\n");
signal(SIGINT, navigator_shutdown);
carmen_map_server_subscribe_compact_cost_map(NULL, (carmen_handler_t) map_server_compact_cost_map_message_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_behavior_selector_subscribe_goal_list_message(NULL, (carmen_handler_t) goal_list_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_behavior_selector_subscribe_current_state_message(NULL, (carmen_handler_t) state_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_localize_ackerman_subscribe_globalpos_message(NULL, (carmen_handler_t)localize_globalpos_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_ipc_dispatch();
return 0;
}
static void add_place(int argc, char *argv[])
{
char *input_filename;
int next_arg;
int force;
char cmd[1024];
char tmp_filename[1024];
carmen_FILE *fp_in, *fp_out;
carmen_map_placelist_t place_list;
carmen_place_p places;
int system_err;
int num_args;
next_arg = handle_options(argc, argv, &force);
next_arg++;
num_args = argc - next_arg;
if(num_args != 4 && num_args != 5 && num_args != 8) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_warn("\nUsage: %s <mapfilename> <placename> <place params>\n",
argv[0]);
carmen_warn(" 2, 3, or 6 place parameters can be given.\n");
}
input_filename = check_mapfile(argv[next_arg]);
next_arg++;
if(!carmen_map_file(input_filename))
carmen_die("Error: %s does not appear to be a valid carmen map file;\n"
"if it is gzipped, make sure it has a \".gz\" extension.\n",
input_filename);
if(!carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK)) {
place_list.num_places = 1;
place_list.places = (carmen_place_p)calloc(1, sizeof(carmen_place_t));
carmen_test_alloc(place_list.places);
} else {
carmen_map_read_places_chunk(input_filename, &place_list);
place_list.num_places++;
place_list.places = (carmen_place_p)realloc
(place_list.places, sizeof(carmen_place_t) * place_list.num_places);
/* check_alloc checked */
}
carmen_test_alloc(place_list.places);
places = place_list.places;
if(num_args == 4) {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_NAMED_POSITION_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
}
else if(num_args == 5) {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_NAMED_POSE_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
places[place_list.num_places - 1].theta =
carmen_degrees_to_radians(atof(argv[next_arg+3]));
fprintf(stderr, "Set (%s %s %s) to (%f.2m, %f.2m, %f.2 rad)\n", argv[4],
argv[5], argv[6], places[place_list.num_places - 1].x,
places[place_list.num_places - 1].y,
places[place_list.num_places - 1].theta);
}
else {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_LOCALIZATION_INIT_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
places[place_list.num_places - 1].theta =
carmen_degrees_to_radians(atof(argv[next_arg+3]));
places[place_list.num_places - 1].x_std = atof(argv[next_arg+4]);
places[place_list.num_places - 1].y_std = atof(argv[next_arg+5]);
places[place_list.num_places - 1].theta_std =
carmen_degrees_to_radians(atof(argv[next_arg+6]));
fprintf(stderr, "Set (%s %s %s %s %s %s) to:\n(%f.2m +/- %f.2m, %f.2m "
"+/- %f.2m, %f.2 +/- %f.2 rad)\n",
argv[next_arg+1], argv[next_arg+2], argv[next_arg+3],
argv[next_arg+4], argv[next_arg+5], argv[next_arg+6],
places[place_list.num_places - 1].x,
places[place_list.num_places - 1].x_std,
places[place_list.num_places - 1].y,
places[place_list.num_places - 1].y_std,
places[place_list.num_places - 1].theta,
places[place_list.num_places - 1].theta_std);
}
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Error: file %s could not be opened for reading",
input_filename);
strcpy(tmp_filename, "/tmp/newmapXXXXXX");
system_err = mkstemp(tmp_filename);
if (system_err == -1)
carmen_die_syserror("I need to create a temporary file in /tmp, but I "
"can't for the\n following reason\n");
fp_out = carmen_fopen(tmp_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Error: file could not be opened for writing");
if(carmen_map_strip(fp_in, fp_out, CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die("Error: could not strip places from map file.\n");
if(carmen_map_write_places_chunk(fp_out, place_list.places,
place_list.num_places) < 0)
carmen_die("Error: could not write places chunk.\n");
carmen_fclose(fp_in);
carmen_fclose(fp_out);
sprintf(cmd, "mv -f %s %s", tmp_filename, input_filename);
system_err = system(cmd);
if (system_err != 0)
carmen_die("I created a temporary file contained the map with the new "
"place name\nat %s. I tried to copy the new file onto your "
"old file, but the copy\nfailed for the following reason: %s\n"
"\n"
"You will have to copy the new file over yourself.\n",
tmp_filename, strerror(errno));
}
static void minimize(int argc, char *argv[])
{
char *input_filename, *output_filename;
int next_arg;
carmen_FILE *in_fp, *out_fp;
int ret_val;
carmen_map_t map;
int x_offset, y_offset;
carmen_offlimits_list_t offlimits_list;
carmen_map_placelist_t places;
int force;
int previous_num_places;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 2) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s minimize <in map filename> <out map filename>\n\n",
argv[0]);
}
input_filename = check_mapfile(argv[next_arg]);
output_filename = check_output(argv[next_arg+1], force);
/*
* Read the gridmap, places and offlimits chunks and minimize them
*/
if (carmen_map_read_gridmap_chunk(input_filename, &map) < 0) {
carmen_die_syserror("Couldn't read GRIDMAP_CHUNK from %s", input_filename);
}
carmen_warn("Map size was %d x %d, ",map.config.x_size, map.config.y_size);
carmen_minimize_gridmap(&map, &x_offset, &y_offset);
carmen_warn("is now %d x %d (offset %d, %d)\n", map.config.x_size,
map.config.y_size, x_offset, y_offset);
ret_val = carmen_map_chunk_exists(input_filename,
CARMEN_MAP_OFFLIMITS_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_offlimits_chunk
(input_filename, &(offlimits_list.offlimits),
&(offlimits_list.list_length));
if (ret_val < 0)
carmen_die_syserror("Couldn't read OFFLIMITS_CHUNK in %s",
input_filename);
carmen_minimize_offlimits(&offlimits_list, x_offset*map.config.resolution,
y_offset*map.config.resolution);
} else
offlimits_list.list_length = 0;
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_places_chunk(input_filename, &places);
if (ret_val < 0)
carmen_die_syserror("Couldn't read PLACES_CHUNK in %s", input_filename);
previous_num_places = places.num_places;
carmen_minimize_places(&places, x_offset*map.config.resolution,
y_offset*map.config.resolution,
map.config.x_size*map.config.resolution,
map.config.y_size*map.config.resolution);
if (places.num_places < previous_num_places)
carmen_warn("%d place locations were dropped from the map after "
"minimization.\n", previous_num_places-places.num_places);
} else
places.num_places = 0;
/*
* Pass everything else through untouched, and then write the rotated
* chunks at the end.
*/
in_fp = carmen_fopen(input_filename, "r");
if (in_fp == NULL)
carmen_die_syserror("Couldn't open %s for reading", input_filename);
out_fp = carmen_fopen(output_filename, "w");
if (out_fp == NULL)
carmen_die_syserror("Couldn't open %s for writing", output_filename);
if (carmen_map_vstrip(in_fp, out_fp, 3, CARMEN_MAP_GRIDMAP_CHUNK,
CARMEN_MAP_OFFLIMITS_CHUNK,
CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Couldn't strip map to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map.map, map.config.x_size,
map.config.y_size,
map.config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap to %s", output_filename);
if (offlimits_list.list_length > 0) {
if (carmen_map_write_offlimits_chunk(out_fp, offlimits_list.offlimits,
offlimits_list.list_length) < 0)
carmen_die_syserror("Couldn't write offlimits list to %s",
output_filename);
}
if (places.num_places > 0) {
if (carmen_map_write_places_chunk(out_fp, places.places,
places.num_places) < 0)
carmen_die_syserror("Couldn't write places list to %s", output_filename);
}
carmen_fclose(in_fp);
carmen_fclose(out_fp);
}
static void rotate(int argc, char *argv[])
{
int force;
char *input_filename, *output_filename;
int next_arg;
carmen_FILE *in_fp, *out_fp;
int ret_val;
carmen_map_t map;
int rotation = 0;
double remain;
int degrees_angle;
carmen_offlimits_list_t offlimits_list;
carmen_map_placelist_t places_list;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s rotate <rotation in degrees> <in map filename> "
"<out map filename>\n\n", argv[0]);
}
degrees_angle = (int)(atof(argv[next_arg]) / 90);
remain = fabs(degrees_angle*90 - atof(argv[next_arg]));
if (carmen_radians_to_degrees(remain) > 2)
carmen_die("Rotations only supported in increments of 90 degrees.\n");
else
rotation = (int)atof(argv[next_arg]) / 90;
input_filename = check_mapfile(argv[next_arg+1]);
output_filename = check_output(argv[next_arg+2], force);
carmen_warn("Rotating by %d degrees\n", rotation*90);
/*
* Read the gridmap, places and offlimits chunks and rotate them
*/
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_GRIDMAP_CHUNK);
if (ret_val < 0)
carmen_die_syserror("Couldn't check existence of GRIDMAP_CHUNK in %s",
input_filename);
if (carmen_map_read_gridmap_chunk(input_filename, &map) < 0)
carmen_die_syserror("Couldn't read GRIDMAP_CHUNK from %s", input_filename);
carmen_rotate_gridmap(&map, rotation);
ret_val = carmen_map_chunk_exists(input_filename,
CARMEN_MAP_OFFLIMITS_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_offlimits_chunk
(input_filename, &(offlimits_list.offlimits),
&(offlimits_list.list_length));
if (ret_val < 0)
carmen_die_syserror("Couldn't read OFFLIMITS_CHUNK in %s",
input_filename);
carmen_rotate_offlimits(map.config, &offlimits_list, rotation);
} else
offlimits_list.list_length = 0;
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_places_chunk(input_filename, &places_list);
if (ret_val < 0)
carmen_die_syserror("Couldn't read PLACES_CHUNK in %s", input_filename);
carmen_rotate_places(map.config, &places_list, rotation);
} else
places_list.num_places = 0;
/*
* Pass everything else through untouched, and then write the rotated
* chunks at the end.
*/
in_fp = carmen_fopen(input_filename, "r");
if (in_fp == NULL)
carmen_die_syserror("Couldn't open %s for reading", input_filename);
out_fp = carmen_fopen(output_filename, "w");
if (out_fp == NULL)
carmen_die_syserror("Couldn't open %s for writing", output_filename);
if (carmen_map_vstrip(in_fp, out_fp, 3, CARMEN_MAP_GRIDMAP_CHUNK,
CARMEN_MAP_OFFLIMITS_CHUNK,
CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Couldn't strip map to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map.map, map.config.x_size,
map.config.y_size,
map.config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap to %s", output_filename);
if (offlimits_list.list_length > 0) {
if (carmen_map_write_offlimits_chunk(out_fp, offlimits_list.offlimits,
offlimits_list.list_length) < 0)
carmen_die_syserror("Couldn't write offlimits list to %s",
output_filename);
}
if (places_list.num_places > 0) {
if (carmen_map_write_places_chunk(out_fp, places_list.places,
places_list.num_places) < 0)
carmen_die_syserror("Couldn't write places list to %s", output_filename);
}
carmen_fclose(in_fp);
carmen_fclose(out_fp);
}
int
main(int argc, char **argv)
{
int num_kinect_devices = 1;
int *flipped=NULL;
double *resolution=NULL;
double *fov=NULL;
double *maxrange=NULL;
char var_name[256];
char var_res[256];
char var_fov[256];
char var_flipped[256];
char var_maxrange[256];
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
carmen_param_t laser_num_devs[] = {{"kinect", "num_kinect_devices", CARMEN_PARAM_INT, &num_kinect_devices, 0, NULL}};
carmen_param_install_params(argc, argv, laser_num_devs, sizeof(laser_num_devs) / sizeof(laser_num_devs[0]));
if (num_kinect_devices<1)
carmen_die("You have to specify at least one kinect device to run laser.\nPlease check you ini file for the parameter num_kinect_devices.\n");
resolution = calloc( num_kinect_devices, sizeof(double) );
carmen_test_alloc(resolution);
fov = calloc( num_kinect_devices, sizeof(double) );
carmen_test_alloc(fov);
flipped = calloc( num_kinect_devices, sizeof(int) );
carmen_test_alloc(flipped);
maxrange = calloc( num_kinect_devices, sizeof(double) );
carmen_test_alloc(maxrange);
for(int kinect_id=0; kinect_id<num_kinect_devices; kinect_id++)
{
sprintf(var_name,"laser%d", to_laser_id(kinect_id));
strcpy(var_fov,var_name);
strcat(var_fov, "_fov");
strcpy(var_res,var_name);
strcat(var_res, "_resolution");
strcpy(var_maxrange,var_name);
strcat(var_maxrange, "_maxrange");
strcpy(var_flipped,var_name);
strcat(var_flipped, "_flipped");
carmen_param_t laser_params[] = {
{"laser", var_fov, CARMEN_PARAM_DOUBLE, &(fov[kinect_id]), 0, NULL},
{"laser", var_res, CARMEN_PARAM_DOUBLE, &(resolution[kinect_id]), 0, NULL},
{"laser", var_maxrange, CARMEN_PARAM_DOUBLE, &(maxrange[kinect_id]), 0, NULL},
{"laser", var_flipped, CARMEN_PARAM_INT, &(flipped[kinect_id]), 0, NULL}};
carmen_param_install_params(argc, argv, laser_params, sizeof(laser_params) / sizeof(laser_params[0]));
init_message_laser_params(to_laser_id(kinect_id), 0.02, fov[kinect_id], resolution[kinect_id], maxrange[kinect_id]);
carmen_laser_define_laser_message(to_laser_id(kinect_id));
carmen_kinect_subscribe_depth_message(kinect_id, NULL,
(carmen_handler_t) ipc_kinect_depth_handler,
CARMEN_SUBSCRIBE_ALL );
}
signal(SIGINT, shutdown_camera_view);
IPC_dispatch();
free(fov);
free(flipped);
free(maxrange);
free(resolution);
return 0;
}
void
carmen_planner_util_test_trajectory(carmen_planner_path_p path)
{
int index;
carmen_ackerman_traj_point_t point = {0,0,0,0,0};
carmen_ackerman_traj_point_p point_p;
carmen_planner_util_clear_path(path);
if (path->length != 0)
carmen_die("Test failed: clear should set length to 0, but length is %d\n",
path->length);
memset(&point, 0, sizeof(carmen_ackerman_traj_point_t));
for (index = 0; index < 100; index++)
{
point.x = index;
point.y = index;
carmen_planner_util_add_path_point(point, path);
}
for (index = 0; index < 100; index++)
{
point_p = carmen_planner_util_get_path_point(index, path);
if (point_p->x != index || point_p->y != index)
carmen_die("Test failed: After 100 set points, get point on %d did not "
"match: was %.0f %.0f\n", index, point_p->x, point_p->y);
}
point.x = 50.5;
point.y = 50.5;
carmen_planner_util_insert_blank(50, path);
carmen_planner_util_set_path_point(50, &point, path);
point_p = carmen_planner_util_get_path_point(50, path);
if (fabs(point_p->x - 50.5) > 0.05 || fabs(point_p->y - 50.5) > 0.05)
carmen_die("Blank then set failed.\n");
if (path->length != 101)
carmen_die("Length (%d) not 101 after insert_blank then set.\n",
path->length);
point.x = 60.5;
point.y = 60.5;
carmen_planner_util_insert_path_point(60, &point, path);
if (fabs(point_p->x - 50.5) > 0.05 || fabs(point_p->y - 50.5) > 0.05)
carmen_die("Blank then set failed.\n");
if (path->length != 102)
carmen_die("Length (%d) not 102 after insert_blank then set.\n",
path->length);
carmen_planner_util_delete_path_point(50, path);
carmen_planner_util_delete_path_point(60, path);
if (path->length != 100)
carmen_die("Length (%d) not 100 after insert_blank then set.\n",
path->length);
carmen_planner_util_clip_path(50, path);
for (index = 0; index < path->length; index++)
carmen_planner_util_delete_path_point(index, path);
for (index = 0; index < path->length; index++)
{
point_p = carmen_planner_util_get_path_point(index, path);
if (point_p->x != 2*index+1 || point_p->y != 2*index+1)
carmen_die("Test failed: After deleting even points, get point on %d "
"(%d) did not match: was %.0f %.0f %.0f\n", 2*index+1, index,
point_p->x, point_p->y,
carmen_radians_to_degrees(point_p->theta));
}
}
//
// This calls the procedures in the other files which do all the real work.
// If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make
// certain to set LOW_DURATION in low.h to some incredibly high number.
//
void *Slam(void * /*arg unused */)
{
TPath *path, *trashPath;
TSenseLog *obs, *trashObs;
double oldmC_D = 0;
double oldmC_T = 0;
double oldvC_D = 0;
double oldvC_T = 0;
double oldmD_D = 0;
double oldmD_T = 0;
double oldvD_D = 0;
double oldvD_T = 0;
double oldmT_D = 0;
double oldmT_T = 0;
double oldvT_D = 0;
double oldvT_T = 0;
outFile = fopen("motionModel.txt", "w");
fprintf(outFile, "This file lists the motion model parameters in the following order : \n");
fprintf(outFile, " - mean value from observed translation\n");
fprintf(outFile, " - mean value from observed rotation\n");
fprintf(outFile, " - variance contributed from observed translation\n");
fprintf(outFile, " - variance contributed from observed rotation\n");
fprintf(outFile, "\n");
fprintf(outFile, "------------Intermediate Models------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fclose(outFile);
while ((fabs(oldmC_D-meanC_D) > 0.01)||(fabs(oldmC_T-meanC_T) > 0.03)||(fabs(oldvC_D-varC_D) > 0.01)||(fabs(oldvC_T-varC_T) > 0.1) ||
(fabs(oldmD_D-meanD_D) > 0.01)||(fabs(oldmD_T-meanD_T) > 0.03)||(fabs(oldvD_D-varD_D) > 0.01)||(fabs(oldvD_T-varD_T) > 0.1) ||
(fabs(oldmT_D-meanT_D) > 0.03)||(fabs(oldmT_T-meanT_T) > 0.01)||(fabs(oldvT_D-varT_D) > 0.03)||(fabs(oldvT_T-varT_T) > 0.01)) {
readFile = carmen_fopen(PLAYBACK, "r");
if(readFile == NULL)
carmen_die("Error: could not open file %s for reading.\n", PLAYBACK);
logfile_index = carmen_logfile_index_messages(readFile);
InitLowSlam();
LowSlam(&path, &obs);
carmen_fclose(readFile);
oldmC_D = meanC_D;
oldmC_T = meanC_T;
oldvC_D = varC_D;
oldvC_T = varC_T;
oldmD_D = meanD_D;
oldmD_T = meanD_T;
oldvD_D = varD_D;
oldvD_T = varD_T;
oldmT_D = meanT_D;
oldmT_T = meanT_T;
oldvT_D = varT_D;
oldvT_T = varT_T;
outFile = fopen("motionModel.txt", "a");
Learn(path);
fclose(outFile);
// Get rid of the path and log of observations
while (path != NULL) {
trashPath = path;
path = path->next;
free(trashPath);
}
while (obs != NULL) {
trashObs = obs;
obs = obs->next;
free(trashObs);
}
}
outFile = fopen("motionModel.txt", "a");
fprintf(outFile, "\n");
fprintf(outFile, "------------Final Model------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fprintf(outFile, "#define meanC_D %.4f\n#define meanC_T %.4f\n#define varC_D %.4f\n#define varC_T %.4f\n\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "#define meanD_D %.4f\n#define meanD_T %.4f\n#define varD_D %.4f\n#define varD_T %.4f\n\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "#define meanT_D %.4f\n#define meanT_T %.4f\n#define varT_D %.4f\n#define varT_T %.4f\n\n", meanT_D, meanT_T, varT_D, varT_T);
fclose(outFile);
CloseLowSlam();
return NULL;
}
int main(int argc, char *argv[])
{
struct stat buf;
double x, y, theta, tv, rv;
double left_disp, right_disp, delta_time;
int fd;
int unused_return_value;
if (argc != 2 || carmen_strncasecmp(argv[1], "-h", 2) == 0 ||
carmen_strncasecmp(argv[1], "--h", 3) == 0) {
if (argc > 2)
carmen_warn("Too many arguments.\n\n");
else if (argc < 2)
carmen_warn("Not enough arguments.\n\n");
carmen_die("Usage: %s <serial dev name>\n", argv[0]);
}
// Does the device exist?
carmen_warn("Device exists test: ");
if (stat(argv[1], &buf) == 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device exists test");
}
// Can we open the device?
carmen_warn("Device open test: ");
fd = open(argv[1], O_RDWR | O_SYNC | O_NOCTTY, 0);
if (fd >= 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device open test");
}
// close(fd);
// Can we talk to the orc board properly?
carmen_warn("Orc recognized test: ");
if (carmen_base_direct_initialize_robot("orc", argv[1]) == 0)
carmen_warn("OK\n");
else {
carmen_die("FAILED\n");
}
// Can we move the wheels?
carmen_warn("Orc drive test: %sMAKE SURE THE ROBOT IS ON BLOCKS%s.\n",
carmen_red_code, carmen_normal_code);
carmen_warn("Hit return to start the orc drive test....");
unused_return_value = scanf("%*c");
if (1) {
// Move left wheel
carmen_warn("Left wheel forwards test: ");
carmen_base_command_velocity(1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. The motors need "
"to be swapped.\n");
if (left_disp < -1e-3)
carmen_die("FAILED\n Left encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Left wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. Very odd.\n");
if (left_disp > 1e-3)
carmen_die("FAILED\n Left encoder moved forwards. Very odd.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
// Move right wheel
carmen_warn("Right wheel forwards test: ");
carmen_base_command_velocity(1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. This is very odd, since it"
"also moved with\nthe other wheel.\n");
if (right_disp < -1e-3)
carmen_die("FAILED\nRight encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Right wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. Very odd.\n");
if (right_disp > 1e-3)
carmen_die("FAILED\nRight encoder moved forwards. Very odd.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
}
carmen_warn("OK\n");
carmen_base_direct_reset();
carmen_warn("Orc drive test: ");
if (carmen_base_direct_set_velocity(1.0, 0.0) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Let some encoder data build up
carmen_warn("Sleep\n");
sleep(1);
carmen_base_direct_set_velocity(0.0, 0.0);
// Are we getting encoder data?
carmen_warn("Orc encoder test: ");
if (carmen_base_direct_get_integrated_state(&x, &y, &theta, &tv, &rv) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Is encoder data valid?
carmen_warn("Orc encoder data validity test: ");
if (fabs(x) > 0 && fabs(y) < 1e-2 &&
fabs(carmen_radians_to_degrees(theta)) < 10)
carmen_warn("OK (%.2f)\n", x);
else
carmen_die("FAILED (%.2f %.2f %.2f)\n", x, y,
carmen_radians_to_degrees(theta));
carmen_base_direct_shutdown_robot();
return 0;
}
static void help(int argc, char **argv)
{
char *action;
if (argc < 3 || argc > 3)
main_usage(argv[0]);
action = argv[2];
if (carmen_strcasecmp(action, "info") == 0)
carmen_die("\nUsage: %s info <map filename>\n\n",
argv[0]);
else if (carmen_strcasecmp(action, "toppm") == 0)
carmen_die("\nUsage: %s toppm <map filename> <ppm filename>\n\n",
argv[0]);
else if (carmen_strcasecmp(action, "tomap") == 0)
carmen_die("\nUsage: %s tomap <resolution> <image file> "
"<map filename>\n\nThe image file can be in most standard "
"formats such as gif, png, jpeg, etc.\n\n", argv[0]);
else if (carmen_strcasecmp(action, "rotate") == 0)
carmen_die("\nUsage: %s rotate <rotation in degrees> <in map filename> "
"<out map filename>\n\nThe rotation angle must be in multiples "
"of 90degrees -- arbitrary rotations are not\nyet supported."
"\n\n", argv[0]);
else if (carmen_strcasecmp(action, "minimize") == 0)
carmen_die("\nUsage: %s minimize <in map filename> <out map filename>\n\n",
argv[0]);
else if (carmen_strcasecmp(action, "add_place") == 0) {
fprintf(stderr, "\nUsage: %s <mapfilename> <placename> <place params>\n",
argv[0]);
fprintf(stderr, " 2, 3, or 6 place parameters can be given.\n");
fprintf(stderr, " 2 parameters - named position (x, y)\n");
fprintf(stderr, " 3 parameters - named pose (x, y, theta)\n");
fprintf(stderr, " 6 parameters - initialization position\n");
fprintf(stderr, " (x, y, theta, std_x, std_y, "
"std_theta)\n");
fprintf(stderr, "\nRemember: The x/y/std_x/std_y co-ordinates are in "
"%sMETRES%s, not in\n", carmen_red_code, carmen_normal_code);
fprintf(stderr, "grid cells. \n");
fprintf(stderr, "\nAlso: theta is in %sDEGREES%s. I will print out the "
"conversion to radians\n", carmen_red_code, carmen_normal_code);
fprintf(stderr, "automatically for you.\n\n");
fprintf(stderr, "Note that unlike rotate, minimize, etc., add_place "
"performs an in-place edit\nto the map, and creates a backup "
"copy.\n\n");
exit(0);
} else if (carmen_strcasecmp(action, "add_offset") == 0) {
carmen_warn("\nUsage: %s <mapfilename> <x> <y> <theta>\n",
argv[0]);
carmen_warn(" <theta> should be given in degrees.\n");
exit(0);
} else if (carmen_strcasecmp(action, "strip") == 0) {
fprintf(stderr, "\nUsage: %s strip <in map filename> <in map filename> "
"<chunk type>\n\n", argv[0]);
fprintf(stderr, "If the map file contains a data chunk of this type, "
"it will\nbe removed on output. All other chunks will be "
"passed through untouched.\n");
fprintf(stderr, "<chunk type> can be one of \"laserscans\", "
"\"places\", \"gridmap\", \"offlimits\", ");
fprintf(stderr, " \"expected\".\n\n");
exit(0);
}
carmen_warn("\nUnrecognized command %s\n", action);
main_usage(argv[0]);
}
carmen_centrallist_p carmen_multicentral_initialize(int argc, char **argv,
void (*exit_handler)(void))
{
FILE *fp = NULL;
carmen_centrallist_p centrallist;
int err2, valid, i, one_central;
char filename[256], *centralhost, *err, line[256];
centrallist = (carmen_centrallist_p)calloc(1, sizeof(carmen_centrallist_t));
carmen_test_alloc(centrallist);
centrallist_copy = centrallist;
valid = 0;
if(argc >= 3)
for(i = 1; i < argc - 1; i++)
if(strcmp(argv[i], "-central") == 0) {
strcpy(filename, argv[i + 1]);
valid = 1;
break;
}
if(valid)
fp = fopen(filename, "r");
if(valid && fp != NULL) {
/* read from the file */
centrallist->num_centrals = 0;
centrallist->central = NULL;
do {
err = fgets(line, 256, fp);
if(err != NULL) {
valid = 0;
for(i = 0; i < (int)strlen(line); i++)
if(isalpha(line[i]))
valid = 1;
if(valid) {
(centrallist->num_centrals)++;
centrallist->central =
(carmen_central_p)realloc(centrallist->central,
centrallist->num_centrals *
sizeof(carmen_central_t));
carmen_test_alloc(centrallist->central);
sscanf(line, "%s",
centrallist->central[centrallist->num_centrals - 1].host);
centrallist->central[centrallist->num_centrals - 1].connected = 0;
centrallist->central[centrallist->num_centrals - 1].ready_for_reconnect = 0;
}
}
} while(err != NULL);
fclose(fp);
}
else {
/* if we can't open the list of centrals, default to connecting to
localhost */
if(valid)
carmen_warn("Error: could not open central list %s for reading.\n",
filename);
centrallist->num_centrals = 1;
centrallist->central = (carmen_central_p)
calloc(1, sizeof(carmen_central_t));
carmen_test_alloc(centrallist->central);
centralhost = getenv("CENTRALHOST");
if(centralhost == NULL)
strcpy(centrallist->central[0].host, "localhost");
else
strcpy(centrallist->central[0].host, centralhost);
centrallist->central[0].connected = 0;
centrallist->central[0].ready_for_reconnect = 0;
}
fprintf(stderr, "CENTRAL List:\n");
fprintf(stderr, "-------------\n");
for(i = 0; i < centrallist->num_centrals; i++)
fprintf(stderr, "%d : %s\n", i, centrallist->central[i].host);
fprintf(stderr, "\n");
/* set verbosity level */
IPC_setVerbosity(IPC_Silent);
/* construct unique IPC module name */
snprintf(module_name, 200, "%s-%d",
carmen_extract_filename(argv[0]), getpid());
/* attempt to connect to each central */
one_central = 0;
for(i = 0; i < centrallist->num_centrals; i++) {
err2 = carmen_multicentral_ipc_connect(module_name,
centrallist->central[i].host);
/* if successful, get IPC context */
if(err2 >= 0) {
fprintf(stderr, "MULTICENTRAL: central %s connected.\n",
centrallist->central[i].host);
centrallist->central[i].connected = 1;
centrallist->central[i].context = IPC_getContext();
x_ipcRegisterExitProc(carmen_multicentral_ipc_exit_handler);
one_central = 1;
}
else
centrallist->central[i].context = NULL;
}
if(!one_central && carmen_allow_no_centrals)
carmen_warn("Error: could not connect to any central.\n");
else if(!one_central)
carmen_die("Error: could not connect to any central.\n");
client_exit_handler = exit_handler;
return centrallist;
}
static void add_new_dot_filter(int *cluster_map, int c, int n,
double *x, double *y) {
int i, id;
double ux, uy, vx, vy, vxy, cnt;
for (id = 0; id < num_filters; id++) {
for (i = 0; i < num_filters; i++)
if (filters[i].id == id)
break;
if (i == num_filters)
break;
}
printf("A%d\n", id);
for (i = 0; i < n && cluster_map[i] != c; i++);
if (i == n)
carmen_die("Error: invalid cluster! (cluster %d not found)\n", c);
num_filters++;
filters = (carmen_dot_filter_p) realloc(filters, num_filters*sizeof(carmen_dot_filter_t));
carmen_test_alloc(filters);
filters[num_filters-1].id = id;
ux = uy = 0.0;
cnt = 0;
for (i = 0; i < n; i++)
if (cluster_map[i] == c) {
ux += x[i];
uy += y[i];
cnt++;
}
ux /= (double)cnt;
uy /= (double)cnt;
vx = vy = vxy = 0.0;
for (i = 0; i < n; i++)
if (cluster_map[i] == c) {
vx += (x[i]-ux)*(x[i]-ux);
vy += (y[i]-uy)*(y[i]-uy);
vxy += (x[i]-ux)*(y[i]-uy);
}
vx /= (double)cnt;
vy /= (double)cnt;
vxy /= (double)cnt;
filters[num_filters-1].person_filter.x = ux;
filters[num_filters-1].person_filter.y = uy;
filters[num_filters-1].person_filter.x0 = ux;
filters[num_filters-1].person_filter.y0 = uy;
for (i = 0; i < MAX_PERSON_FILTER_VELOCITY_WINDOW; i++) {
filters[num_filters-1].person_filter.vx[i] = 0.0;
filters[num_filters-1].person_filter.vy[i] = 0.0;
}
filters[num_filters-1].person_filter.vpos = 0;
filters[num_filters-1].person_filter.vlen = 0;
filters[num_filters-1].person_filter.hidden_cnt = 0;
filters[num_filters-1].person_filter.px = vx; //default_person_filter_px;
filters[num_filters-1].person_filter.py = vy; //default_person_filter_py;
filters[num_filters-1].person_filter.pxy = vxy; //default_person_filter_pxy;
filters[num_filters-1].person_filter.a = default_person_filter_a;
filters[num_filters-1].person_filter.qx = default_person_filter_qx;
filters[num_filters-1].person_filter.qy = default_person_filter_qy;
filters[num_filters-1].person_filter.qxy = default_person_filter_qxy;
filters[num_filters-1].person_filter.rx = default_person_filter_rx;
filters[num_filters-1].person_filter.ry = default_person_filter_ry;
filters[num_filters-1].person_filter.rxy = default_person_filter_rxy;
for (i = 0; i < n; i++)
if (cluster_map[i] == c) {
//printf(" (%.2f, %.2f)", x[i], y[i]);
person_filter_sensor_update(&filters[num_filters-1].person_filter, x[i], y[i]);
}
//printf("\n");
filters[num_filters-1].trash_filter.x = ux;
filters[num_filters-1].trash_filter.y = uy;
filters[num_filters-1].trash_filter.px = vx; //default_trash_filter_px;
filters[num_filters-1].trash_filter.py = vy; //default_trash_filter_py;
filters[num_filters-1].trash_filter.py = vxy;
filters[num_filters-1].trash_filter.a = default_trash_filter_a;
filters[num_filters-1].trash_filter.qx = default_trash_filter_qx;
filters[num_filters-1].trash_filter.qy = default_trash_filter_qy;
filters[num_filters-1].trash_filter.qxy = default_trash_filter_qxy;
filters[num_filters-1].trash_filter.rx = default_trash_filter_rx;
filters[num_filters-1].trash_filter.ry = default_trash_filter_ry;
filters[num_filters-1].trash_filter.rxy = default_trash_filter_rxy;
for (i = 0; i < n; i++)
if (cluster_map[i] == c)
trash_filter_sensor_update(&filters[num_filters-1].trash_filter, x[i], y[i]);
filters[num_filters-1].door_filter.x = ux;
filters[num_filters-1].door_filter.y = uy;
filters[num_filters-1].door_filter.t = bnorm_theta(vx, vy, vxy);
// bnorm_theta(filters[num_filters-1].person_filter.px,
// filters[num_filters-1].person_filter.py,
// filters[num_filters-1].person_filter.pxy);
filters[num_filters-1].door_filter.px = vx; //default_door_filter_px;
filters[num_filters-1].door_filter.py = vy; //default_door_filter_py;
filters[num_filters-1].door_filter.pt = default_door_filter_pt;
filters[num_filters-1].door_filter.pxy = vxy; //0; //dbug?
filters[num_filters-1].door_filter.a = default_door_filter_a;
filters[num_filters-1].door_filter.qx = default_door_filter_qx;
filters[num_filters-1].door_filter.qy = default_door_filter_qy;
filters[num_filters-1].door_filter.qxy = default_door_filter_qxy;
filters[num_filters-1].door_filter.qt = default_door_filter_qt;
for (i = 0; i < n; i++)
if (cluster_map[i] == c)
door_filter_sensor_update(&filters[num_filters-1].door_filter, x[i], y[i]);
filters[num_filters-1].type = dot_classify(&filters[num_filters-1]);
filters[num_filters-1].allow_change = 1;
filters[num_filters-1].sensor_update_cnt = 0;
filters[num_filters-1].do_motion_update = 0;
filters[num_filters-1].updated = 1;
filters[num_filters-1].last_type = filters[num_filters-1].type;
filters[num_filters-1].invisible = 0;
filters[num_filters-1].invisible_cnt = 0;
print_filters();
}
int
main( int argc, char *argv[] )
{
carmen_point_t pos, corrpos, odo;
double time;
int i;
int scancnt;
carmen_move_t pos_move = {0.0, 0.0, 0.0};
carmen_move_t corr_move = {0.0, 0.0, 0.0};
carmen_point_t old_pos = {0.0, 0.0, 0.0};
carmen_point_t old_corrpos = {0.0, 0.0, 0.0};
int which_laser = 0;
carmen_FILE stdout_carmen;
stdout_carmen.compressed = 0;
stdout_carmen.fp = stdout;
if (argc < 2) {
print_usage();
exit(1);
}
scancnt=0;
for (i=1; i < argc-1; i++) {
if (!strcmp(argv[i], "-f") || !strcmp(argv[i], "--flaser"))
which_laser = 1;
if (!strcmp(argv[i], "-r") || !strcmp(argv[i], "--robotlaser"))
which_laser = 2;
}
carmen_ipc_initialize(argc, argv);
vascocore_init(argc, argv);
/* file handler */
carmen_FILE *logfile = NULL;
/* log file index structure */
carmen_logfile_index_p logfile_index = NULL;
/* open logfile for reading */
logfile = carmen_fopen(argv[argc-1], "r");
if(logfile == NULL)
carmen_die("Error: could not open file %s for reading.\n", argv[1]);
/* create index structure */
logfile_index = carmen_logfile_index_messages(logfile);
/* buffer for reading the lines */
const int max_line_length=9999;
char line[max_line_length+1];
/* used to read the messages. erase stucture before! */
carmen_robot_laser_message l;
carmen_erase_structure(&l, sizeof(carmen_robot_laser_message) );
/* iterate over all entries in the logfile */
while (!carmen_logfile_eof(logfile_index)) {
int correct_msg = 1;
/* read the line from the file */
carmen_logfile_read_next_line(logfile_index,
logfile,
max_line_length,
line);
/* what kind of message it this? */
if (which_laser != 1 && strncmp(line, "ROBOTLASER1 ", 12) == 0) {
/* convert the string using the corresponding read function */
char* next = carmen_string_to_robot_laser_message(line, &l);
time = atof( strtok( next, " ") );
}
else if (which_laser != 2 && strncmp(line, "FLASER ", 7) == 0) {
/* convert the string using the corresponding read function */
char* next = carmen_string_to_robot_laser_message_orig(line, &l);
time = atof( strtok( next, " ") );
}
else {
correct_msg = 0;
fputs(line, stdout);
}
if (correct_msg) {
odo = l.robot_pose;
pos = l.laser_pose;
/* using the new scan match function that takes robot laser messages...
carmen_laser_laser_message scan;
carmen_erase_structure(&scan, sizeof(carmen_laser_laser_message) );
scan.timestamp = l.timestamp;
scan.config = l.config;
scan.num_readings = l.num_readings;
scan.num_remissions = l.num_remissions;
scan.range = l.range;
scan.remission = l.remission;
scan.host = l.host;
corrpos = vascocore_scan_match( scan, pos );*/
corrpos = vascocore_scan_match_robot( l );
scancnt++;
corr_move = carmen_move_between_points( old_corrpos, corrpos );
pos_move = carmen_move_between_points( old_pos, pos );
fprintf( stderr, "expected movement %.6f m %.6f m %.6f degree\n",
corr_move.forward, corr_move.sideward,
rad2deg(corr_move.rotation) );
fprintf( stderr, "corrected movement %.6f m %.6f m %.6f degree\n\n",
pos_move.forward, pos_move.sideward,
rad2deg(pos_move.rotation) );
l.laser_pose = corrpos;
carmen_logwrite_write_robot_laser(&l,1,&stdout_carmen, time);
fflush(stdout);
old_pos = pos;
old_corrpos = corrpos;
}
}
/* close the file */
carmen_fclose(logfile);
fprintf(stderr, "\ndone, corrected %d messages!\n", scancnt);
/* free index structure */
carmen_logfile_free_index(&logfile_index);
return(0);
}
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);
}
int main(int argc, char **argv)
{
int i, j, found, vrml_obj;
char filename[200], vrml_filename[200];
vrml_file_t vrml_file;
if(argc < 2)
carmen_die("Error: missing arguments\n"
"Usage: %s filename\n", argv[0]);
strcpy(filename, argv[1]);
strcpy(vrml_filename, filename);
if(strcmp(filename + strlen(filename) - 7, ".glv.gz") == 0)
strcpy(vrml_filename + strlen(vrml_filename) - 6, "vrml.gz");
else if(strcmp(filename + strlen(filename) - 4, ".glv") == 0)
strcpy(vrml_filename + strlen(vrml_filename) - 3, "vrml.gz");
else
carmen_die("Error: file must end in .glv or .glv.gz\n");
/* open the output file */
vrml_file_open(&vrml_file, vrml_filename);
/* read the glv model */
obj = glv_object_read(argv[1]);
/* allocate the color list */
color_list = (glv_color_p)calloc(max_colors, sizeof(glv_color_t));
carmen_test_alloc(color_list);
/* make a list of the point colors */
for(i = 0; i < obj->num_points; i++) {
found = 0;
for(j = 0; j < num_colors; j++)
if(same_color(obj->point[i].c, color_list[j]))
found = 1;
if(!found) {
if(num_colors == max_colors) {
max_colors += 10;
color_list = (glv_color_p)realloc(color_list,
max_colors * sizeof(glv_color_t));
}
color_list[num_colors] = obj->point[i].c;
num_colors++;
}
}
/* write the point list objects to the vrml file */
fprintf(stderr, "Converting points (%d colors) : ", num_colors);
for(i = 0; i < num_colors; i++) {
vrml_obj = vrml_file_new_object(&vrml_file, VRML_POINTSET,
color_list[i].r / 255.0,
color_list[i].g / 255.0,
color_list[i].b / 255.0);
for(j = 0; j < obj->num_points; j++)
if(obj->point[j].c.r == color_list[i].r &&
obj->point[j].c.g == color_list[i].g &&
obj->point[j].c.b == color_list[i].b)
vrml_add_point(&vrml_file, vrml_obj, obj->point[j].x,
obj->point[j].y, obj->point[j].z);
vrml_file_close_object(&vrml_file, vrml_obj);
fprintf(stderr, "%d ", i + 1);
}
fprintf(stderr, "\n");
/* start over with the colors */
num_colors = 0;
/* make a list of the line colors */
for(i = 0; i < obj->num_lines; i++) {
found = 0;
for(j = 0; j < num_colors; j++)
if(same_color(obj->line[i].c, color_list[j]))
found = 1;
if(!found) {
if(num_colors == max_colors) {
max_colors += 10;
color_list = (glv_color_p)realloc(color_list,
max_colors * sizeof(glv_color_t));
}
color_list[num_colors] = obj->line[i].c;
num_colors++;
}
}
/* write the line list objects to the vrml file */
fprintf(stderr, "Converting lines (%d colors) : ", num_colors);
for(i = 0; i < num_colors; i++) {
vrml_obj = vrml_file_new_object(&vrml_file, VRML_LINESET,
color_list[i].r / 255.0,
color_list[i].g / 255.0,
color_list[i].b / 255.0);
for(j = 0; j < obj->num_lines; j++)
if(obj->line[j].c.r == color_list[i].r &&
obj->line[j].c.g == color_list[i].g &&
obj->line[j].c.b == color_list[i].b)
vrml_add_line(&vrml_file, vrml_obj,
obj->line[j].p1.x,
obj->line[j].p1.y,
obj->line[j].p1.z,
obj->line[j].p2.x,
obj->line[j].p2.y,
obj->line[j].p2.z);
vrml_file_close_object(&vrml_file, vrml_obj);
fprintf(stderr, "%d ", i + 1);
}
fprintf(stderr, "\n");
/* start over with the colors */
num_colors = 0;
/* make a list of the line colors */
for(i = 0; i < obj->num_faces; i++) {
found = 0;
for(j = 0; j < num_colors; j++)
if(same_color(obj->face[i].c, color_list[j]))
found = 1;
if(!found) {
if(num_colors == max_colors) {
max_colors += 10;
color_list = (glv_color_p)realloc(color_list,
max_colors * sizeof(glv_color_t));
}
color_list[num_colors] = obj->face[i].c;
num_colors++;
}
}
/* write the line list objects to the vrml file */
fprintf(stderr, "Converting faces (%d colors) : ", num_colors);
for(i = 0; i < num_colors; i++) {
if(color_list[i].r == 255 && color_list[i].g == 255 &&
color_list[i].b == 255)
vrml_obj = vrml_file_new_object(&vrml_file, VRML_FACESET,
-1, -1, -1);
else
vrml_obj = vrml_file_new_object(&vrml_file, VRML_FACESET,
color_list[i].r / 255.0,
color_list[i].g / 255.0,
color_list[i].b / 255.0);
for(j = 0; j < obj->num_faces; j++)
if(obj->face[j].c.r == color_list[i].r &&
obj->face[j].c.g == color_list[i].g &&
obj->face[j].c.b == color_list[i].b) {
vrml_add_face(&vrml_file, vrml_obj,
obj->face[j].p1.x,
obj->face[j].p1.y,
obj->face[j].p1.z,
obj->face[j].p2.x,
obj->face[j].p2.y,
obj->face[j].p2.z,
obj->face[j].p3.x,
obj->face[j].p3.y,
obj->face[j].p3.z);
}
vrml_file_close_object(&vrml_file, vrml_obj);
fprintf(stderr, "%d ", i + 1);
}
fprintf(stderr, "\n");
vrml_file_close(&vrml_file);
return 0;
}
static void add_offset(int argc, char *argv[])
{
char *input_filename;
int next_arg;
int force;
char cmd[1024];
char tmp_filename[1024];
carmen_FILE *fp_in, *fp_out;
carmen_global_offset_t offset;
int system_err;
int num_args;
int response;
next_arg = handle_options(argc, argv, &force);
next_arg++;
num_args = argc - next_arg;
if(num_args != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_warn("\nUsage: %s <mapfilename> <x> <y> <theta>\n",
argv[0]);
carmen_warn(" <theta> should be given in degrees.\n");
}
input_filename = check_mapfile(argv[next_arg]);
next_arg++;
if(!carmen_map_file(input_filename))
carmen_die("Error: %s does not appear to be a valid carmen map file;\n"
"if it is gzipped, make sure it has a \".gz\" extension.\n",
input_filename);
if(carmen_map_chunk_exists(input_filename, CARMEN_MAP_GLOBAL_OFFSET_CHUNK)) {
carmen_warn("Offset chunk exists already. Replace? [y/n] ");
response = getchar();
if (tolower(response) != 'y')
exit(0);
carmen_warn("Replacing...\n");
}
offset.x = atof(argv[next_arg]);
offset.y = atof(argv[next_arg+1]);
offset.theta = carmen_degrees_to_radians(atof(argv[next_arg+2]));
fprintf(stderr, "Set (%s %s %s) to (%.2f m, %.2f m, %.2f rad)\n",
argv[next_arg], argv[next_arg+1], argv[next_arg+2],
offset.x, offset.y, offset.theta);
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Error: file %s could not be opened for reading",
input_filename);
strcpy(tmp_filename, "/tmp/newmapXXXXXX");
system_err = mkstemp(tmp_filename);
if (system_err == -1)
carmen_die_syserror("I need to create a temporary file in /tmp, but I "
"can't for the\n following reason\n");
fp_out = carmen_fopen(tmp_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Error: file could not be opened for writing");
if(carmen_map_strip(fp_in, fp_out, CARMEN_MAP_GLOBAL_OFFSET_CHUNK) < 0)
carmen_die("Error: could not strip global offset from map file.\n");
if(carmen_map_write_global_offset_chunk(fp_out, &offset) < 0)
carmen_die("Error: could not write offset chunk.\n");
carmen_fclose(fp_in);
carmen_fclose(fp_out);
sprintf(cmd, "mv -f %s %s", tmp_filename, input_filename);
system_err = system(cmd);
if (system_err != 0)
carmen_die("I created a temporary file contained the map with the new "
"place name\nat %s. I tried to copy the new file onto your "
"old file, but the copy\nfailed for the following reason: %s\n"
"\n"
"You will have to copy the new file over yourself.\n",
tmp_filename, strerror(errno));
}
int main(int argc, char *argv[]) {
UDPConnectionServer connection(2368);
AcquisitionThread<DataPacket> thread(connection);
Calibration calib;
std::shared_ptr<DataPacket> packet;
IPC_RETURN_TYPE err;
char dump_filename[4096];
carmen_velodyne_ipc_initialize(argc, argv);
carmen_velodyne_read_parameters(argc, argv);
carmen_velodyne_set_spin_rate(spin_rate);
calib_file.open(calib_path);
if (!calib_file.is_open())
carmen_die("\nError: Could not open %s for reading\n", calib_filename);
calib_file >> calib;
calib_file.close();
signal(SIGINT, carmen_velodyne_sigint_handler);
thread.start();
double time;
double start_time = carmen_get_time();
int num_packets = 0;
int start_num_packets = 0;
int num_lost_packets = 0;
int start_num_lost_packets = 0;
while (!quit) {
try {
while (dump_enabled || points_publish) {
packet = thread.getBuffer().dequeue();
++num_packets;
if (dump_enabled) {
if (!dump_file.is_open()) {
char dump_time[1024];
time_t local = packet->getTimestamp();
struct tm* time = localtime(&local);
strftime(dump_time, sizeof(dump_time), "%Y-%m-%d-%H%M%S", time);
sprintf(dump_filename, "%s/%s.bin", dump_path, dump_time);
dump_file.open(dump_filename, std::ios::out | std::ios::binary);
}
if (dump_file.is_open()) {
long dump_filepos = dump_file.tellp();
dump_file << *packet;
carmen_velodyne_publish_packet(id, dump_filename,
dump_filepos, packet->getTimestamp());
}
else {
carmen_warn("\nWarning: Could not open %s for writing\n",
dump_filename);
}
}
if (points_publish) {
VdynePointCloud pointCloud;
Converter::toPointCloud(*packet, calib, pointCloud);
int num_points = pointCloud.getSize();
float x[num_points], y[num_points], z[num_points];
std::vector<VdynePointCloud::Point3D>::const_iterator it;
int i = 0;
for (it = pointCloud.getPointBegin();
it != pointCloud.getPointEnd(); ++it, ++i) {
x[i] = it->mX;
y[i] = it->mY;
z[i] = it->mZ;
}
carmen_velodyne_publish_pointcloud(id, num_points, x, y, z,
packet->getTimestamp());
}
double time = carmen_get_time();
if (time-start_time >= 1.0) {
num_lost_packets = thread.getBuffer().getNumDroppedElements();
float period_num_packets = num_packets-start_num_packets;
float period_num_lost_packets = num_lost_packets-
start_num_lost_packets;
double packet_loss = period_num_lost_packets/
(period_num_packets+period_num_lost_packets);
fprintf(stdout, "\rPacket loss: %6.2f%%", packet_loss*100.0);
fflush(stdout);
start_time = time;
start_num_packets = num_packets;
start_num_lost_packets = num_lost_packets;
}
}
}
catch (IOException& exception) {
// buffer underrun
}
carmen_ipc_sleep(0.0);
}
if (dump_file.is_open())
dump_file.close();
fprintf(stdout, "\n");
return 0;
}
static void info(int argc, char *argv[])
{
time_t creation_time;
char *filename;
char username[100], origin[100], description[100];
int chunk_size, i;
carmen_map_config_t config;
carmen_map_placelist_t place_list;
carmen_offlimits_t *offlimits_list;
int list_length;
carmen_global_offset_t offset;
/* Check for the appropriate command line argument */
if(argc != 3)
carmen_die("Error in info: wrong number of parameters.\n");
filename = argv[2];
/* Make sure the file exists */
if (!carmen_map_file(filename))
carmen_die("Error: %s does not appear to be a valid carmen map file;\n"
"if it is gzipped, make sure it has a \".gz\" extension.\n"
"Do you have an incompatible file?\n"
"Current map version: %s\n", filename, CARMEN_MAP_VERSION);
/* Print the CREATOR information if the chunk exists */
if(carmen_map_read_creator_chunk(filename, &creation_time,
username, origin, description) == 0) {
printf("\nMap CREATOR information:\n");
printf("------------------------\n");
printf("Map author : %s\n", username);
printf("Creation date : %s", asctime(localtime(&creation_time)));
printf("Origin : %s\n", origin);
printf("Description : %s", description);
printf("\n");
}
/* Print the list of chunks in the file */
printf("Map contains the following CHUNK types:\n");
printf("---------------------------------------\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_CREATOR_CHUNK);
if(chunk_size > 0)
printf("CREATOR : yes (%d bytes)\n", chunk_size);
else
printf("CREATOR : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_GRIDMAP_CHUNK);
if(chunk_size > 0)
{
carmen_map_read_gridmap_config(filename, &config);
printf("GRIDMAP : yes size : %4d x %4d, resolution : %.2f m/cell (%d bytes)\n",
config.x_size, config.y_size, config.resolution, chunk_size);
}
else
printf("GRIDMAP : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_OFFLIMITS_CHUNK);
if(chunk_size > 0) {
printf("OFFLIMITS : yes (%d bytes)\n", chunk_size);
carmen_map_read_offlimits_chunk(filename, &offlimits_list, &list_length);
for (i = 0; i < list_length; i++) {
if (offlimits_list[i].type == CARMEN_OFFLIMITS_POINT_ID)
carmen_warn("\tOfflimits %d: point: %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1);
else if (offlimits_list[i].type == CARMEN_OFFLIMITS_LINE_ID)
carmen_warn("\tOfflimits %d: line: %d %d %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1, offlimits_list[i].x2, offlimits_list[i].y2);
else if (offlimits_list[i].type == CARMEN_OFFLIMITS_RECT_ID)
carmen_warn("\tOfflimits %d: rect: %d %d %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1, offlimits_list[i].x2, offlimits_list[i].y2);
}
}
else
printf("OFFLIMITS : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_PLACES_CHUNK);
if(chunk_size > 0)
printf("PLACES : yes (%d bytes)\n", chunk_size);
else
printf("PLACES : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_EXPECTED_CHUNK);
if(chunk_size > 0)
printf("EXPECTED : yes (%d bytes)\n", chunk_size);
else
printf("EXPECTED : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_LASERSCANS_CHUNK);
if(chunk_size > 0)
printf("LASERSCANS : yes (%d bytes)\n", chunk_size);
else
printf("LASERSCANS : no\n");
if(carmen_map_chunk_exists(filename, CARMEN_MAP_PLACES_CHUNK)) {
printf("\nMap contains the following places:\n");
printf("----------------------------------\n");
carmen_map_read_places_chunk(filename, &place_list);
for(i = 0; i < place_list.num_places; i++)
if(place_list.places[i].type == CARMEN_NAMED_POSITION_TYPE)
printf("%-20s (%.1f m, %.1f m)\n", place_list.places[i].name,
place_list.places[i].x, place_list.places[i].y);
else if(place_list.places[i].type == CARMEN_NAMED_POSE_TYPE)
printf("%-20s (%.1f m, %.1f m, %.1f deg)\n",
place_list.places[i].name, place_list.places[i].x,
place_list.places[i].y,
carmen_radians_to_degrees(place_list.places[i].theta));
else if(place_list.places[i].type == CARMEN_LOCALIZATION_INIT_TYPE)
printf("%-20s (%.1f m, %.1f m, %.1f deg) std = (%.2f m, %.2f m, "
"%.2f deg)\n", place_list.places[i].name,
place_list.places[i].x, place_list.places[i].y,
carmen_radians_to_degrees(place_list.places[i].theta),
place_list.places[i].x_std, place_list.places[i].y_std,
carmen_radians_to_degrees(place_list.places[i].theta_std));
}
chunk_size = carmen_map_chunk_exists(filename,
CARMEN_MAP_GLOBAL_OFFSET_CHUNK);
if (chunk_size > 0) {
carmen_map_read_global_offset_chunk(filename, &offset);
printf("OFFSET : %10.3f %10.3f %10.3f rads\n", offset.x,
offset.y, offset.theta);
} else
printf("OFFSET : no\n");
}
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);
}
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;
}
int main(int argc, char **argv)
{
FILE *index_file;
char index_file_name[2000];
memset(&odometry_ackerman, 0, sizeof(odometry_ackerman));
memset(&velocity_ackerman, 0, sizeof(velocity_ackerman));
memset(&visual_odometry, 0, sizeof(visual_odometry));
memset(&imu, 0, sizeof(imu));
memset(&truepos_ackerman, 0, sizeof(truepos_ackerman));
memset(&laser_ackerman1, 0, sizeof(laser_ackerman1));
memset(&laser_ackerman2, 0, sizeof(laser_ackerman2));
memset(&laser_ackerman3, 0, sizeof(laser_ackerman3));
memset(&laser_ackerman4, 0, sizeof(laser_ackerman4));
memset(&laser_ackerman5, 0, sizeof(laser_ackerman5));
memset(&laser_ldmrs, 0, sizeof(laser_ldmrs));
memset(&laser_ldmrs_objects, 0, sizeof(laser_ldmrs_objects));
memset(&laser_ldmrs_objects_data, 0, sizeof(laser_ldmrs_objects_data));
memset(&rawlaser1, 0, sizeof(rawlaser1));
memset(&rawlaser2, 0, sizeof(rawlaser2));
memset(&rawlaser3, 0, sizeof(rawlaser3));
memset(&rawlaser4, 0, sizeof(rawlaser4));
memset(&rawlaser5, 0, sizeof(rawlaser5));
memset(&gpsgga, 0, sizeof(gpsgga));
memset(&gpshdt, 0, sizeof(gpshdt));
memset(&gpsrmc, 0, sizeof(gpsrmc));
memset(&raw_depth_kinect_0, 0, sizeof(raw_depth_kinect_0));
memset(&raw_depth_kinect_1, 0, sizeof(raw_depth_kinect_1));
memset(&raw_video_kinect_0, 0, sizeof(raw_video_kinect_0));
memset(&raw_video_kinect_1, 0, sizeof(raw_video_kinect_1));
memset(&velodyne_partial_scan, 0, sizeof(velodyne_partial_scan));
memset(&velodyne_variable_scan, 0, sizeof(velodyne_variable_scan));
memset(&velodyne_gps, 0, sizeof(velodyne_gps));
memset(&xsens_euler, 0, sizeof(xsens_euler));
memset(&xsens_quat, 0, sizeof(xsens_quat));
memset(&xsens_matrix, 0, sizeof(xsens_matrix));
memset(&xsens_mtig, 0, sizeof(xsens_mtig));
memset(&bumblebee_basic_stereoimage1, 0, sizeof(bumblebee_basic_stereoimage1));
memset(&bumblebee_basic_stereoimage2, 0, sizeof(bumblebee_basic_stereoimage2));
memset(&bumblebee_basic_stereoimage3, 0, sizeof(bumblebee_basic_stereoimage3));
memset(&bumblebee_basic_stereoimage4, 0, sizeof(bumblebee_basic_stereoimage4));
memset(&bumblebee_basic_stereoimage5, 0, sizeof(bumblebee_basic_stereoimage5));
memset(&bumblebee_basic_stereoimage6, 0, sizeof(bumblebee_basic_stereoimage6));
memset(&bumblebee_basic_stereoimage7, 0, sizeof(bumblebee_basic_stereoimage7));
memset(&bumblebee_basic_stereoimage8, 0, sizeof(bumblebee_basic_stereoimage8));
memset(&bumblebee_basic_stereoimage9, 0, sizeof(bumblebee_basic_stereoimage9));
memset(&web_cam_message, 0, sizeof(web_cam_message));
memset(&ackerman_motion_message, 0, sizeof(ackerman_motion_message));
memset(&ultrasonic_message, 0, sizeof(ultrasonic_message));
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
register_ipc_messages ();
read_parameters (argc, argv);
carmen_playback_define_messages ();
signal(SIGINT, shutdown_playback_module);
logfile = carmen_fopen(argv[1], "r");
if (logfile == NULL)
carmen_die("Error: could not open file %s for reading.\n", argv[1]);
int fadvise_error = posix_fadvise(fileno(logfile->fp), 0, 0, POSIX_FADV_SEQUENTIAL);
if (fadvise_error)
{
printf("Could not advise POSIX_FADV_SEQUENTIAL on playback\n");
exit(1);
}
strcpy(index_file_name, argv[1]);
strcat(index_file_name, ".index");
index_file = fopen(index_file_name, "r");
if (index_file == NULL)
{
logfile_index = carmen_logfile_index_messages(logfile);
save_logindex_file(logfile_index, index_file_name);
}
else if (index_file_older_than_log_file(index_file, logfile))
{
fclose(index_file);
logfile_index = carmen_logfile_index_messages(logfile);
save_logindex_file(logfile_index, index_file_name);
}
else
{
logfile_index = load_logindex_file(index_file_name);
}
main_playback_loop();
return 0;
}
static int
read_parameters_from_file(void)
{
FILE *fp = NULL;
char *line;
char *mark, *token;
int token_num;
char lvalue[255], rvalue[MAX_VARIABLE_LENGTH];
int found_matching_robot = 0;
int found_desired_robot = 0;
int expert = 0;
int line_length;
int count;
fp = fopen(param_filename, "r");
if(fp == NULL)
return -1;
line = (char *)calloc(MAX_VARIABLE_LENGTH, sizeof(char));
carmen_test_alloc(line);
count = 0;
while (!feof(fp)) {
fgets(line, MAX_VARIABLE_LENGTH-1, fp);
line[MAX_VARIABLE_LENGTH-1] = '\0';
if (strlen(line) == MAX_VARIABLE_LENGTH-1)
carmen_die("Line %d of file %s is too long.\n"
"Maximum line length is %d. Please correct this line.\n\n"
"It is also possible that this file has become corrupted.\n"
"Make sure you have an up-to-date version of carmen, and\n"
"consult the param_server documentation to make sure the\n"
"file format is valid.\n", count, param_filename,
MAX_VARIABLE_LENGTH-1);
count++;
if (feof(fp))
break;
mark = strchr(line, '#'); /* strip comments and trailing returns */
if (mark != NULL)
mark[0] = '\0';
mark = strchr(line, '\n');
if (mark != NULL)
mark[0] = '\0';
// Trim off trailing white space
line_length = strlen(line) - 1;
while (line_length >= 0 &&
(line[line_length] == ' ' || line[line_length] == '\t' )) {
line[line_length--] = '\0';
}
line_length++;
if (line_length == 0)
continue;
// Skip over initial blank space
mark = line + strspn(line, " \t");
if (strlen(mark) == 0)
carmen_die("You have encountered a bug in carmen. Please report it\n"
"to the carmen maintainers. \n"
"Line %d, function %s, file %s\n", __LINE__, __FUNCTION__,
__FILE__);
token_num = 0;
/* tokenize line */
token = mark;
// Move mark to the first whitespace character.
mark = strpbrk(mark, " \t");
// If we found a whitespace character, then turn it into a NULL
// and move mark to the next non-whitespace.
if (mark) {
mark[0] = '\0';
mark++;
mark += strspn(mark, " \t");
}
if (strlen(token) > 254) {
carmen_warn("Bad file format of %s on line %d.\n"
"The parameter name %s is too long (%d characters).\n"
"A parameter name can be no longer than 254 "
"characters.\nSkipping this line.\n", param_filename,
count, token, (int) strlen(token));
continue;
}
strcpy(lvalue, token);
token_num++;
// If mark points to a non-whitespace character, then we have a
// two-token line
if (mark) {
if (strlen(mark) > MAX_VARIABLE_LENGTH-1) {
carmen_warn("Bad file format of %s on line %d.\n"
"The parameter value %s is too long (%d "
"characters).\nA parameter value can be no longer "
"than %u characters.\nSkipping this line.\n",
param_filename, count, mark, (int) strlen(mark),
MAX_VARIABLE_LENGTH-1);
continue;
}
strcpy(rvalue, mark);
token_num++;
}
if (lvalue[0] == '[') {
if (strcspn(lvalue+1,"]") == 6 && !carmen_strncasecmp(lvalue+1, "expert", 6)) {
found_matching_robot = 1;
expert = 1;
}
else {
expert = 0;
if (lvalue[1] == '*')
found_matching_robot = 1;
else if (selected_robot) {
if (strlen(lvalue) < strlen(selected_robot) + 2)
found_matching_robot = 0;
else if (lvalue[strlen(selected_robot)+1] != ']')
found_matching_robot = 0;
else if (carmen_strncasecmp
(lvalue+1, selected_robot, strlen(selected_robot)) == 0) {
found_matching_robot = 1;
found_desired_robot = 1;
} else
found_matching_robot = 0;
}
}
}
else if(token_num == 2 && found_matching_robot == 1)
set_param(lvalue, rvalue, expert);
} /* End of while (!feof(fp)) */
fclose(fp);
if (selected_robot && !found_desired_robot) {
carmen_die("Did not find a match for robot %s. Do you have the right\n"
"init file? (Reading parameters from %s)\n", selected_robot,
param_filename);
}
return 0;
}
//parses a reading response
void hokuyo_parseReading(HokuyoRangeReading* r, char* buffer, carmen_laser_laser_type_t laser_type){
char* s=buffer;
int expectedStatus=0;
if (s[0]=='M')
expectedStatus=99;
if (s[0]=='C')
expectedStatus=00;
int beamBytes=0;
if (s[1]=='D')
beamBytes=3;
if (s[0]=='C') {
beamBytes = 3;
//printf("got C\n");
}
if (s[1] == 'S') {
beamBytes = 2;
// printf("got C\n");
}
// printf("s[0] = %c s[1] = %c\n",s[0],s[1]);
if (! beamBytes || ! expectedStatus){
fprintf(stderr, "Invalid return packet, cannot parse reading\n");
r->status=-1;
return;
}
s+=2; // jump to "Starting Step"
char v[5];
v[4]='\0';
strncpy(v,s,4); r->startStep=atoi(v); s+=4; // grab StartStep and jump to EndStep
strncpy(v,s,4); r->endStep=atoi(v); s+=4; // grab EndStep and jump to ClusterCount
v[2]='\0'; strncpy(v,s,2); r->clusterCount=atoi(v);
//printf("startStep = %i, endStep = %i\n",r->startStep, r->endStep);
s=skipLine(s); // skip over rest of line (ScanInterval, NumberOfScans, StringCharacters, and LF
if (s==0){
fprintf(stderr, "error, line broken when reading the range parameters\n");
r->status=-1;
return;
}
// When the status is 99: The second line starts with '99' (status),
// followed by a 'b', a LF and the Hokuyo TimeStamp
strncpy(v,s,2); r->status=atoi(v); s+=2;
if (r->status==expectedStatus){
} else {
fprintf(stderr,"Error, Status=%d",r->status);
return;
}
r->timestamp=parseInt(4,&s);
s=skipLine(s); // skip over the rest of the line (Sum and LF)
// The remaining lines begin with a 64B data block, followed by a sum and LF
int i=0;
int max_beams =0;
if (laser_type==HOKUYO_URG)
max_beams=URG_MAX_BEAMS;
else if (laser_type==HOKUYO_UTM)
max_beams=UTM_MAX_BEAMS;
else{
carmen_die("UNKNOWN hokuyo type!\n");
}
while(s!='\0'){
if ( i>(max_beams)){
printf("broke because of max_beams!\n"); //shouldn't happen
break;
}
//r->ranges[i++]=parseInt(beamBytes,&s);
r->ranges[i]=parseInt(beamBytes,&s);
i++;
}
i--; //i is 1 more than the number we've actually read
r->n_ranges=i;
}
static void
get_sensors_param(int argc, char **argv)
{
int i, j;
int flipped;
int horizontal_resolution;
char stereo_velodyne_string[256];
int stereo_velodyne_vertical_roi_ini;
int stereo_velodyne_vertical_roi_end;
int stereo_velodyne_horizontal_roi_ini;
int stereo_velodyne_horizontal_roi_end;
int roi_ini, roi_end;
spherical_sensor_params[0].pose = velodyne_pose;
spherical_sensor_params[0].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[0].pose.position, sensor_board_1_to_car_matrix);
spherical_sensor_params[0].height = spherical_sensor_params[0].sensor_robot_reference.z + robot_wheel_radius;
if (spherical_sensor_params[0].height > highest_sensor)
highest_sensor = spherical_sensor_params[0].height;
if (spherical_sensor_params[0].alive && !strcmp(spherical_sensor_params[0].name,"velodyne"))
{
spherical_sensor_params[0].ray_order = carmen_velodyne_get_ray_order();
spherical_sensor_params[0].vertical_correction = carmen_velodyne_get_vertical_correction();
spherical_sensor_params[0].delta_difference_mean = carmen_velodyne_get_delta_difference_mean();
spherical_sensor_params[0].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev();
carmen_param_t param_list[] =
{
{spherical_sensor_params[0].name, (char*)"vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[0].vertical_resolution, 0, NULL},
{(char *)"localize_ackerman", (char*)"velodyne_range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].range_max, 0, NULL},
{spherical_sensor_params[0].name, (char*)"time_spent_by_each_scan", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[0].time_spent_by_each_scan, 0, NULL},
};
carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));
init_velodyne_points(&spherical_sensor_data[0].points, &spherical_sensor_data[0].intensity, &spherical_sensor_data[0].robot_pose,
&spherical_sensor_data[0].robot_velocity, &spherical_sensor_data[0].robot_timestamp, &spherical_sensor_data[0].robot_phi);
spherical_sensor_params[0].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[0].pose.orientation);
spherical_sensor_data[0].point_cloud_index = 0;
carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[0], spherical_sensor_params[0].vertical_resolution, number_of_threads);
if (max_range < spherical_sensor_params[0].range_max)
{
max_range = spherical_sensor_params[0].range_max;
}
spherical_sensor_params[0].current_range_max = spherical_sensor_params[0].range_max;
}
for (i = 1; i < number_of_sensors; i++)
{
if (spherical_sensor_params[i].alive)
{
spherical_sensor_params[i].pose = get_stereo_velodyne_pose_3D(argc, argv, i);
spherical_sensor_params[i].sensor_robot_reference = carmen_change_sensor_reference(sensor_board_1_pose.position, spherical_sensor_params[i].pose.position, sensor_board_1_to_car_matrix);
spherical_sensor_params[i].height = spherical_sensor_params[i].sensor_robot_reference.z + robot_wheel_radius;
if (spherical_sensor_params[i].height > highest_sensor)
highest_sensor = spherical_sensor_params[i].height;
sprintf(stereo_velodyne_string, "%s%d", "stereo", i);
carmen_param_t param_list[] =
{
{spherical_sensor_params[i].name, (char*) "vertical_resolution", CARMEN_PARAM_INT, &spherical_sensor_params[i].vertical_resolution, 0, NULL},
{spherical_sensor_params[i].name, (char*) "horizontal_resolution", CARMEN_PARAM_INT, &horizontal_resolution, 0, NULL},
{spherical_sensor_params[i].name, (char*) "flipped", CARMEN_PARAM_ONOFF, &flipped, 0, NULL},
{spherical_sensor_params[i].name, (char*) "range_max", CARMEN_PARAM_DOUBLE, &spherical_sensor_params[i].range_max, 0, NULL},
{spherical_sensor_params[i].name, (char*) "vertical_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_ini, 0, NULL },
{spherical_sensor_params[i].name, (char*) "vertical_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_vertical_roi_end, 0, NULL },
{spherical_sensor_params[i].name, (char*) "horizontal_roi_ini", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_ini, 0, NULL },
{spherical_sensor_params[i].name, (char*) "horizontal_roi_end", CARMEN_PARAM_INT, &stereo_velodyne_horizontal_roi_end, 0, NULL }
};
carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));
if (flipped)
{
spherical_sensor_params[i].vertical_resolution = horizontal_resolution;
roi_ini = stereo_velodyne_horizontal_roi_ini;
roi_end = stereo_velodyne_horizontal_roi_end;
}
else
{
roi_ini = stereo_velodyne_vertical_roi_ini;
roi_end = stereo_velodyne_vertical_roi_end;
}
if (spherical_sensor_params[i].vertical_resolution > (roi_end - roi_ini))
{
carmen_die("The stereo_velodyne_vertical_resolution is bigger than stereo point cloud height");
}
if (max_range < spherical_sensor_params[i].range_max)
{
max_range = spherical_sensor_params[i].range_max;
}
spherical_sensor_params[i].current_range_max = spherical_sensor_params[i].range_max;
spherical_sensor_params[i].range_max_factor = 1.0;
spherical_sensor_params[i].ray_order = generates_ray_order(spherical_sensor_params[i].vertical_resolution);
spherical_sensor_params[i].vertical_correction = get_variable_velodyne_correction();
// spherical_sensor_params[i].vertical_correction = get_stereo_velodyne_correction(flipped, i, spherical_sensor_params[i].vertical_resolution, roi_ini, roi_end, 0, 0);
init_velodyne_points(&spherical_sensor_data[i].points, &spherical_sensor_data[i].intensity, &spherical_sensor_data[i].robot_pose,
&spherical_sensor_data[i].robot_velocity, &spherical_sensor_data[i].robot_timestamp, &spherical_sensor_data[i].robot_phi);
spherical_sensor_params[i].sensor_to_support_matrix = create_rotation_matrix(spherical_sensor_params[i].pose.orientation);
spherical_sensor_data[i].point_cloud_index = 0;
carmen_prob_models_alloc_sensor_data(&spherical_sensor_data[i], spherical_sensor_params[i].vertical_resolution, number_of_threads);
//TODO : tem que fazer esta medida para as cameras igual foi feito para o velodyne
if(i == 8) {
spherical_sensor_params[i].delta_difference_mean = carmen_velodyne_get_delta_difference_mean();
spherical_sensor_params[i].delta_difference_stddev = carmen_velodyne_get_delta_difference_stddev();
} else {
spherical_sensor_params[i].delta_difference_mean = (double *)calloc(50, sizeof(double));
spherical_sensor_params[i].delta_difference_stddev = (double *)calloc(50, sizeof(double));
for (j = 0; j < 50; j++)
spherical_sensor_params[i].delta_difference_stddev[j] = 1.0;
}
}
}
}
int
main(int argc, char **argv)
{
int width = 0, height = 0;
double resolution = 0.0;
char *filename = NULL;
carmen_map_t map;
int index;
carmen_FILE *out_file;
carmen_read_commandline_parameters(argc, argv);
if (argc > 1)
{
if (strcmp(argv[argc-1], "-w") != 0 &&
strcmp(argv[argc-1], "-h") != 0 &&
strcmp(argv[argc-2], "-w") != 0 &&
strcmp(argv[argc-2], "-h") != 0)
filename = argv[argc-1];
}
carmen_process_param_int("w", usage, &width);
carmen_process_param_int("h", usage, &height);
carmen_process_param_double("r", usage, &resolution);
if (width <= 0)
{
printf("Map width: ");
scanf("%d", &width);
}
if (height <= 0)
{
printf("Map height: ");
scanf("%d", &height);
}
if (resolution <= 0)
{
printf("Map resolution: ");
scanf("%lf", &resolution);
}
if (filename == NULL)
{
printf("Filename: ");
filename = (char *)calloc(1024, sizeof(char));
carmen_test_alloc(filename);
scanf("%s", filename);
}
printf("Creating map %d x %d, resolution %.2f, storing in %s\n",
width, height, resolution, filename);
map.config.x_size = width;
map.config.y_size = height;
map.config.resolution = resolution;
map.config.map_name = filename;
map.complete_map = (float *)calloc(width*height, sizeof(float));
carmen_test_alloc(map.complete_map);
for (index = 0; index < width*height; index++)
map.complete_map[index] = -1;
map.map = (float **)calloc(width, sizeof(float *));
carmen_test_alloc(map.map);
for (index = 0; index < width; index++)
map.map[index] = map.complete_map+index*height;
out_file = carmen_fopen(filename, "w");
if (out_file == NULL)
carmen_die("Couldn't open %s for writing map: %s\n", filename,
strerror(errno));
carmen_map_write_id(out_file);
carmen_map_write_creator_chunk(out_file, "generate_blank", "blank map");
carmen_map_write_gridmap_chunk(out_file, map.map, width, height, resolution);
carmen_fclose(out_file);
return 0;
}
int main(int argc, char **argv)
{
char filename[1024];
char key;
int num_scanned;
/* initialize connection to IPC network */
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
/* open logfile, check if file overwrites something */
if(argc < 2)
carmen_die("usage: %s <logfile>\n", argv[0]);
sprintf(filename, "%s", argv[1]);
outfile = carmen_fopen(filename, "r");
if (outfile != NULL) {
fprintf(stderr, "Overwrite %s? ", filename);
num_scanned = scanf("%c", &key);
if (toupper(key) != 'Y')
exit(-1);
carmen_fclose(outfile);
}
outfile = carmen_fopen(filename, "w");
if(outfile == NULL)
carmen_die("Error: Could not open file %s for writing.\n", filename);
carmen_logwrite_write_header(outfile);
get_logger_params(argc, argv);
if ( !(log_odometry && log_laser && log_robot_laser ) )
carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
if (log_params)
get_all_params();
register_ipc_messages();
if (log_odometry)
carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
base_odometry_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_sonars)
carmen_base_subscribe_sonar_message(NULL, (carmen_handler_t)
base_sonar_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_bumpers)
carmen_base_subscribe_bumper_message(NULL, (carmen_handler_t)
base_bumper_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_arm)
carmen_arm_subscribe_state_message(NULL, (carmen_handler_t)
arm_state_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_pantilt) {
carmen_pantilt_subscribe_scanmark_message (NULL, (carmen_handler_t)
pantilt_scanmark_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_pantilt_subscribe_laserpos_message (NULL, (carmen_handler_t)
pantilt_laserpos_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_robot_laser) {
carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
robot_frontlaser_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
robot_rearlaser_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_laser) {
carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
laser_laser1_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
laser_laser2_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
laser_laser3_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
laser_laser4_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
laser_laser5_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_localize) {
carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
localize_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_simulator) {
carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)
carmen_simulator_truepos_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_imu) {
carmen_imu_subscribe_imu_message(NULL, (carmen_handler_t)
imu_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_gps) {
carmen_gps_subscribe_nmea_message( NULL,
(carmen_handler_t) ipc_gps_gpgga_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_gps_subscribe_nmea_rmc_message( NULL,
(carmen_handler_t) ipc_gps_gprmc_handler,
CARMEN_SUBSCRIBE_ALL );
}
if (log_motioncmds) {
carmen_robot_subscribe_vector_move_message( NULL,
(carmen_handler_t) robot_vector_move_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_robot_subscribe_follow_trajectory_message( NULL,
(carmen_handler_t) robot_follow_trajectory_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_robot_subscribe_velocity_message( NULL,
(carmen_handler_t) robot_velocity_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_base_subscribe_velocity_message( NULL,
(carmen_handler_t) base_velocity_handler,
CARMEN_SUBSCRIBE_ALL );
}
carmen_logger_subscribe_comment_message( NULL,
(carmen_handler_t) logger_comment_handler,
CARMEN_SUBSCRIBE_ALL );
signal(SIGINT, shutdown_module);
logger_starttime = carmen_get_time();
carmen_ipc_dispatch();
return 0;
}
void publish_xsens_global(xsens_global data, /*CmtOutputMode& mode,*/ CmtOutputSettings& settings)
{
IPC_RETURN_TYPE err;
switch (settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {
case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
carmen_xsens_global_quat_message xsens_quat_message;
//Acceleration
xsens_quat_message.m_acc.x = data.acc.x;
xsens_quat_message.m_acc.y = data.acc.y;
xsens_quat_message.m_acc.z = data.acc.z;
//Gyro
xsens_quat_message.m_gyr.x = data.gyr.x;
xsens_quat_message.m_gyr.y = data.gyr.y;
xsens_quat_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_quat_message.m_mag.x = data.mag.x;
xsens_quat_message.m_mag.y = data.mag.y;
xsens_quat_message.m_mag.z = data.mag.z;
xsens_quat_message.quat_data.m_data[0] = data.quatData.data[0];
xsens_quat_message.quat_data.m_data[1] = data.quatData.data[1];
xsens_quat_message.quat_data.m_data[2] = data.quatData.data[2];
xsens_quat_message.quat_data.m_data[3] = data.quatData.data[3];
//Temperature and Message Count
xsens_quat_message.m_temp = data.temp;
xsens_quat_message.m_count = data.count;
//Timestamp
xsens_quat_message.timestamp = carmen_get_time();
//Host
xsens_quat_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_QUAT_NAME, &xsens_quat_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_QUAT_NAME);
break;
case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
carmen_xsens_global_euler_message xsens_euler_message;
//Acceleration
xsens_euler_message.m_acc.x = data.acc.x;
xsens_euler_message.m_acc.y = data.acc.y;
xsens_euler_message.m_acc.z = data.acc.z;
//Gyro
xsens_euler_message.m_gyr.x = data.gyr.x;
xsens_euler_message.m_gyr.y = data.gyr.y;
xsens_euler_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_euler_message.m_mag.x = data.mag.x;
xsens_euler_message.m_mag.y = data.mag.y;
xsens_euler_message.m_mag.z = data.mag.z;
xsens_euler_message.euler_data.m_pitch = data.eulerData.pitch;
xsens_euler_message.euler_data.m_roll = data.eulerData.roll;
xsens_euler_message.euler_data.m_yaw = data.eulerData.yaw;
//Temperature and Message Count
xsens_euler_message.m_temp = data.temp;
xsens_euler_message.m_count = data.count;
//Timestamp
xsens_euler_message.timestamp = carmen_get_time();
//Host
xsens_euler_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_EULER_NAME, &xsens_euler_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_EULER_NAME);
break;
case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
carmen_xsens_global_matrix_message xsens_matrix_message;
//Acceleration
xsens_matrix_message.m_acc.x = data.acc.x;
xsens_matrix_message.m_acc.y = data.acc.y;
xsens_matrix_message.m_acc.z = data.acc.z;
//Gyro
xsens_matrix_message.m_gyr.x = data.gyr.x;
xsens_matrix_message.m_gyr.y = data.gyr.y;
xsens_matrix_message.m_gyr.z = data.gyr.z;
//Magnetism
xsens_matrix_message.m_mag.x = data.mag.x;
xsens_matrix_message.m_mag.y = data.mag.y;
xsens_matrix_message.m_mag.z = data.mag.z;
xsens_matrix_message.matrix_data.m_data[0][0] = data.matrixData.data[0][0];
xsens_matrix_message.matrix_data.m_data[0][1] = data.matrixData.data[0][1];
xsens_matrix_message.matrix_data.m_data[0][2] = data.matrixData.data[0][2];
xsens_matrix_message.matrix_data.m_data[1][0] = data.matrixData.data[1][0];
xsens_matrix_message.matrix_data.m_data[1][1] = data.matrixData.data[1][1];
xsens_matrix_message.matrix_data.m_data[1][2] = data.matrixData.data[1][2];
xsens_matrix_message.matrix_data.m_data[2][0] = data.matrixData.data[2][0];
xsens_matrix_message.matrix_data.m_data[2][1] = data.matrixData.data[2][1];
xsens_matrix_message.matrix_data.m_data[2][2] = data.matrixData.data[2][2];
//Temperature and Message Count
xsens_matrix_message.m_temp = data.temp;
xsens_matrix_message.m_count = data.count;
//Timestamp
xsens_matrix_message.timestamp = carmen_get_time();
xsens_matrix_message.timestamp2 = data.timestamp;
//Host
xsens_matrix_message.host = carmen_get_host();
err = IPC_publishData(CARMEN_XSENS_GLOBAL_MATRIX_NAME, &xsens_matrix_message);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_XSENS_GLOBAL_MATRIX_NAME);
break;
default:
carmen_die("Unknown settings in publish_xsens_global()\n");
break;
}
}
