static void toppm(int argc, char *argv[])
{
int err;
int next_arg;
int force;
char *input_filename, *output_filename;
carmen_map_t map;
next_arg = handle_options(argc, argv, &force);
if(argc - next_arg != 3) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s toppm <map filename> <ppm filename>\n\n",
argv[0]);
}
input_filename = check_mapfile(argv[next_arg+1]);
output_filename = check_output(argv[next_arg+2], force);
if(carmen_map_read_gridmap_chunk(input_filename, &map) < 0)
carmen_die("Error: %s did not contain a gridmap.\n", input_filename);
carmen_map_read_offlimits_chunk_into_map(input_filename, &map);
err = carmen_map_write_to_ppm(&map, output_filename);
if (err < 0)
carmen_die_syserror("Could not write ppm to %s", output_filename);
}
int
carmen_grid_mapping_read_complete_map_type(char *map_path, carmen_map_t *map, char *map_type)
{
FILE *file;
char global_map_path[1000], aux[1000];
int rtr;
if (map_type[0] == 'u'){
map_name = "%scomplete_map_sum.map";
map_name_info = "%scomplete_map_sum.info";
}else if(map_type[0] == 'o'){
map_name = "%scomplete_map_count.map";
map_name_info = "%scomplete_map_count.info";
}else if(map_type[0] == 'e'){
map_name = "%scomplete_map_mean.map";
map_name_info = "%scomplete_map_mean.info";
}
sprintf(global_map_path, map_name, map_path);
printf("Reading map %s\n", global_map_path);
rtr = carmen_map_read_gridmap_chunk(global_map_path, map);
sprintf(global_map_path, map_name_info, map_path);
file = fopen(global_map_path, "r");
if (file == NULL)
fprintf(stderr, "Error: complete map not found!\n");
fscanf(file, "%s\n", aux);
map->config.x_origin = atof(aux);
fscanf(file, "%s\n", aux);
map->config.y_origin = atof(aux);
fclose(file);
return rtr;
}
int
main(int argc, char **argv)
{
carmen_point_t min_pose, max_pose;
FILE *file;
char full_path[1000] = "", global_map_path[1000];
DIR *dp;
struct dirent *dirp;
int i;
carmen_map_t block_map, complete_map;
double x_origin, y_origin, size = 0.0;
char complete_map_name[1024];
// map_type = (char *) malloc(1024);
// strcpy(map_type, "mmmmmmm");
carmen_ipc_initialize(argc, argv);
read_parameters(argc, argv);
min_pose.x = DBL_MAX;
min_pose.y = DBL_MAX;
max_pose.x = -DBL_MAX;
max_pose.y = -DBL_MAX;
dp = opendir(map_path);
sprintf(complete_map_name, "rm %s/complete_map.*", map_path);
// system(complete_map_name);
for (dirp = readdir(dp), i = 0; dirp != NULL; dirp = readdir(dp))
{
if (dirp->d_name[0] != map_type[0])
continue;
strcat(full_path, map_path);
strcat(full_path, "/");//mudar para / no linux
strcat(full_path, dirp->d_name);
get_map_origin_by_filename(full_path, &x_origin, &y_origin);
if (i == 0)
{
carmen_map_read_gridmap_chunk(full_path, &block_map);
size = floor((double)block_map.config.x_size * block_map.config.resolution);
free(block_map.complete_map);
free(block_map.map);
}
if (x_origin < min_pose.x)
min_pose.x = x_origin;
if ((x_origin + size) > max_pose.x)
max_pose.x = x_origin + size;
if (y_origin < min_pose.y)
min_pose.y = y_origin;
if ((y_origin + size) > max_pose.y)
max_pose.y = y_origin + size;
full_path[0] = '\0';
i++;
}
closedir(dp);
complete_map.config.resolution = map_resolution;
complete_map.config.x_origin = min_pose.x;
complete_map.config.y_origin = min_pose.y;
complete_map.config.x_size = (max_pose.x - min_pose.x) / complete_map.config.resolution;
complete_map.config.y_size = (max_pose.y - min_pose.y) / complete_map.config.resolution;
complete_map.complete_map = (double*) malloc(sizeof(double) * complete_map.config.x_size * complete_map.config.y_size);
complete_map.map = (double**)malloc(sizeof(double*) * complete_map.config.x_size);
for (int x = 0; x < complete_map.config.x_size; x++)
{
complete_map.map[x] = &complete_map.complete_map[x * complete_map.config.y_size];
//initializing map with unknown
for (int y = 0; y < complete_map.config.y_size; y++)
{
complete_map.map[x][y] = -1.0;
}
}
dp = opendir(map_path);
for(dirp = readdir(dp), i = 0; dirp != NULL; dirp = readdir(dp))
{
if (dirp->d_name[0] != map_type[0])
continue;
strcat(full_path, map_path);
strcat(full_path, "/");
strcat(full_path, dirp->d_name);
carmen_map_read_gridmap_chunk(full_path, &block_map);
// printf("block map read = %s \n",full_path);
get_map_origin_by_filename(full_path, &block_map.config.x_origin, &block_map.config.y_origin);
build_complete_map(&block_map, &complete_map);
free(block_map.complete_map);
free(block_map.map);
full_path[0] = '\0';
}
closedir(dp);
if (map_type[0] == 'u'){
map_name = "%s/complete_map_sum.map";
map_name_info = "%s/complete_map_sum.info";
}else if(map_type[0] == 'o'){
map_name = "%s/complete_map_count.map";
map_name_info = "%s/complete_map_count.info";
}else if(map_type[0] == 'e'){
map_name = "%s/complete_map_mean.map";
map_name_info = "%s/complete_map_mean.info";
}else if(map_type[0] == 's'){
map_name = "%s/complete_map_remission.map";
map_name_info = "%s/complete_map_remission.info";
}else if(map_type[0] == 'm'){
map_name = "%s/complete_map.map";
map_name_info = "%s/complete_map.info";
}
sprintf(global_map_path, map_name, map_path);
carmen_grid_mapping_save_map(global_map_path, &complete_map);
sprintf(global_map_path, map_name_info, map_path);
file = fopen(global_map_path, "w");
fprintf(file, "%f\n%f\n", complete_map.config.x_origin, complete_map.config.y_origin);
fclose(file);
return 1;
}
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);
}
