static void tomap(int argc, char *argv[])
{
int next_arg;
int force;
double resolution;
char *input_filename, *output_filename;
carmen_map_t *map;
carmen_FILE *out_fp;
char buf[1024];
gtk_init (&argc, &argv);
next_arg = handle_options(argc, argv, &force);
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s tomap <resolution> <ppm filename> "
"<map filename>\n\n", argv[0]);
}
resolution = (double)atof(argv[next_arg+1]);
if (resolution == 0)
carmen_die("%s translated to a resolution of 0.\n"
"A positive, non-zero resolution is required.\n",
argv[next_arg+1]);
input_filename = argv[next_arg+2];
if(!carmen_file_exists(input_filename))
carmen_die("Image file %s does not exist.\n", input_filename);
output_filename = check_output(argv[next_arg+3], force);
map = carmen_map_imagefile_to_map(input_filename, resolution);
out_fp = carmen_fopen(output_filename, "w");
if (carmen_map_write_id(out_fp) < 0)
carmen_die_syserror("Couldn't write map id to %s", output_filename);
sprintf(buf, "Created from %s", input_filename);
if (carmen_map_write_creator_chunk(out_fp, "img_to_map", buf) < 0)
carmen_die_syserror("Couldn't write creator chunk 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 chunk to %s", output_filename);
carmen_fclose(out_fp);
}
int
main(int argc, char **argv)
{
char *in_filename, *out_filename;
carmen_FILE *out_fp;
char buf[1024];
carmen_map_p map;
if (argc != 3)
carmen_die("Usage: %s <bee_filename> <carmen_filename>\n", argv[0]);
in_filename = argv[1];
out_filename = argv[2];
if (carmen_file_exists(out_filename))
{
printf("Overwrite %s? ", out_filename);
scanf("%s", buf);
if (tolower(buf[0]) != 'y')
exit(0);
}
map = read_beesoft_map(in_filename);
out_fp = carmen_fopen(out_filename, "w");
if (out_fp == NULL)
carmen_die("Couldn't open %s for writing : %s\n", out_filename,
strerror(errno));
if (carmen_map_write_id(out_fp) < 0)
carmen_die_syserror("Couldn't write map id to %s", out_filename);
sprintf(buf, "Created from %s", out_filename);
if (carmen_map_write_creator_chunk(out_fp, "bee2carmen", buf) < 0)
carmen_die_syserror("Couldn't write creator chunk to %s", out_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 chunk to %s", out_filename);
carmen_fclose(out_fp);
return 0;
}
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 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;
}
