static char *check_mapfile(char *filename)
{
if(!carmen_file_exists(filename))
carmen_die("Map file %s does not exist.\n", filename);
if (!carmen_map_file(filename))
carmen_die("File %s is not a recognizable map file.\n", filename);
return filename;
}
static int
read_commandline(int argc, char **argv)
{
int index, i;
int cur_arg;
char **robot_names = NULL;
int num_robots = 0;
int binary;
// extern char *optarg;
// extern int optind;
// extern int optopt;
static struct option long_options[] = {
{"help", 0, 0, 0},
{"robot", 1, NULL, 0},
{"alphabetize", 0, &alphabetize, 1},
{0, 0, 0, 0}
};
int option_index = 0;
int c;
char arg_buffer[255];
opterr = 0;
while (1) {
c = getopt_long (argc, argv, "ahr:",
long_options, &option_index);
if (c == -1)
break;
if (c == 0) {
sprintf(arg_buffer, "--%s", long_options[option_index].name);
if (carmen_strcasecmp(long_options[option_index].name, "help") == 0)
c = 'h';
else if (carmen_strcasecmp(long_options[option_index].name,"robot") == 0)
c = 'r';
} else {
sprintf(arg_buffer, "-%c", c);
}
switch (c) {
case 'r':
if (optarg == NULL)
usage(argv[0], "%s requires an argument", arg_buffer);
if (strlen(optarg) > MAX_ROBOT_NAME_LENGTH)
usage(argv[0], "argument to %s: %s is invalid (too long).",
arg_buffer, optarg);
selected_robot = (char *)calloc(strlen(optarg)+1, sizeof(char));
carmen_test_alloc(selected_robot);
strcpy(selected_robot, optarg);
break;
case 'a':
alphabetize = 1;
break;
case 'h':
help(argv[0]);
break;
case ':':
usage(argv[0], "-%c requires an argument", optopt);
break;
case '?':
// usage(argv[0], "unknown option character %c", optopt);
break;
}
}
#ifndef COMPILE_WITHOUT_MAP_SUPPORT
/* look for a map file */
map_filename = NULL;
for (cur_arg = optind; cur_arg < argc; cur_arg++) {
if (carmen_map_file(argv[cur_arg]) && map_filename)
usage(argv[0], "too many map files given: %s and %s", map_filename,
argv[cur_arg]);
if (carmen_map_file(argv[cur_arg])) {
map_filename = (char *)calloc(strlen(argv[cur_arg])+1, sizeof(char));
carmen_test_alloc(map_filename);
strcpy(map_filename, argv[cur_arg]);
}
}
#endif
param_filename = NULL;
for (cur_arg = optind; cur_arg < argc; cur_arg++) {
#ifndef COMPILE_WITHOUT_MAP_SUPPORT
if (map_filename && strcmp(map_filename, argv[cur_arg]) == 0)
continue;
#endif
if (param_filename && carmen_file_exists(argv[cur_arg])) {
usage(argv[0], "Too many ini files given: %s and %s",
param_filename, argv[cur_arg]);
}
if (!carmen_file_exists(argv[cur_arg]))
usage(argv[0], "No such file: %s", argv[cur_arg]);
binary = contains_binary_chars(argv[cur_arg]);
if (binary < 0)
usage(argv[0], "Couldn't read from %s: %s", argv[cur_arg],
strerror(errno));
if (binary > 0)
usage(argv[0], "Invalid ini file %s: (not a valid map file either)",
argv[cur_arg]);
param_filename = (char *)calloc(strlen(argv[cur_arg])+1, sizeof(char));
carmen_test_alloc(param_filename);
strcpy(param_filename, argv[cur_arg]);
}
if (!param_filename) {
for (index = 0; default_list[index]; index++) {
if (carmen_file_exists(default_list[index])) {
param_filename = default_list[index];
break;
}
}
}
robot_names = (char **)calloc(MAX_ROBOTS, sizeof(char *));
carmen_test_alloc(robot_names);
find_valid_robots(robot_names, &num_robots, MAX_ROBOTS);
if (num_robots == 0) {
carmen_warn("Loading parameters for default robot using param file "
"[31;1m%s[0m\n", param_filename);
return 0;
}
robot_names = (char **)realloc(robot_names, num_robots * sizeof(char *));
carmen_test_alloc(robot_names);
if (selected_robot == NULL && num_robots > 1) {
for (cur_arg = 1; cur_arg < optind; cur_arg++) {
for (i = 0; i < num_robots; i++)
if(strcmp(robot_names[i], argv[cur_arg] + 1) == 0) {
selected_robot = argv[cur_arg] + 1;
break;
}
}
}
if (selected_robot == NULL && num_robots > 1)
usage(argv[0], "The ini_file %s contains %d robot definitions.\n"
"You must specify a robot name on the command line using --robot.",
param_filename, num_robots);
if (selected_robot == NULL)
selected_robot = carmen_new_string("%s", robot_names[0]);
for(i = 0; i < num_robots; i++)
free(robot_names[i]);
free(robot_names);
carmen_warn("Loading parameters for robot [31;1m%s[0m using param file "
"[31;1m%s[0m\n", selected_robot, param_filename);
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");
}
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));
}
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));
}
