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); }