static void strip(int argc, char *argv[])
{
char *input_filename, *output_filename, *chunk_type;
carmen_FILE *fp_in, *fp_out;
int next_arg;
int force;
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 strip <in map filename> "
"<out map filename> <chunk type>\n\n", argv[0]);
}
input_filename = check_mapfile(argv[next_arg]);
output_filename = check_output(argv[next_arg+1], force);
chunk_type = argv[next_arg+2];
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Could not open %s for reading", input_filename);
fp_out = carmen_fopen(output_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Could not open %s for writing", output_filename);
if (carmen_strcasecmp(chunk_type, "laserscans") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_LASERSCANS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "gridmap") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_GRIDMAP_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "offlimits") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_OFFLIMITS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "expected") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_EXPECTED_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "places") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
carmen_fclose(fp_in);
carmen_fclose(fp_out);
}
static int
query_num_params(char *module_name) {
int count;
int index;
count = 0;
for (index = 0; index < num_params; index++)
if (carmen_strcasecmp(param_list[index].module_name, module_name) == 0)
count++;
return count;
}
static int
lookup_name(char *full_name)
{
int index;
for (index = 0; index < num_params; index++)
{
if (carmen_strcasecmp(param_list[index].lvalue, full_name) == 0)
return index;
}
return -1;
}
void read_parameters(int argc, char **argv)
{
carmen_param_t camera_params[] = {
{"camera", "dev", CARMEN_PARAM_STRING, &camera_dev, 0, NULL},
{"camera", "image_width", CARMEN_PARAM_INT, &image_width, 0, NULL},
{"camera", "image_height", CARMEN_PARAM_INT, &image_height, 0, NULL},
{"camera", "brightness", CARMEN_PARAM_INT, &brightness, 0, NULL},
{"camera", "hue", CARMEN_PARAM_INT, &hue, 0, NULL},
{"camera", "saturation", CARMEN_PARAM_INT, &saturation, 0, NULL},
{"camera", "contrast", CARMEN_PARAM_INT, &contrast, 0, NULL},
{"camera", "gamma", CARMEN_PARAM_INT, &camera_gamma, 0, NULL},
{"camera", "denoisestrength", CARMEN_PARAM_INT, &denoisestrength, 0, NULL},
{"camera", "awbmode", CARMEN_PARAM_STRING, &awbmode, 0, NULL},
{"camera", "antiflicker", CARMEN_PARAM_ONOFF, &antiflicker, 0, NULL},
{"camera", "backlightcompensation", CARMEN_PARAM_ONOFF, &backlightcompensation, 0, NULL},
{"camera", "useautosharpen", CARMEN_PARAM_ONOFF, &useautosharpen, 0, NULL},
{"camera", "useautoshutter", CARMEN_PARAM_ONOFF, &useautoshutter, 0, NULL},
{"camera", "useagc", CARMEN_PARAM_ONOFF, &useagc, 0, NULL},
{"camera", "fps", CARMEN_PARAM_INT, &fps, 0, NULL}};
carmen_param_install_params(argc, argv, camera_params,
sizeof(camera_params) /
sizeof(camera_params[0]));
carmen_param_set_module("camera");
if (!useautosharpen) {
if (carmen_param_get_int("sharpenstrength", &sharpenstrength,NULL) < 0)
carmen_die("camera_sharpenstrength must be specified or "
"carmen_useautosharpen\nmust be turned on\n");
}
if (!useautoshutter) {
if (carmen_param_get_int("shutterlength", &shutterlength,NULL) < 0)
carmen_die("camera_shutterlength must be specified or "
"carmen_useautoshutter\nmust be turned on\n");
}
if (!useagc) {
if (carmen_param_get_int("gain", &gain,NULL) < 0)
carmen_die("camera_gain must be specified or carmen_useagc\n"
"must be turned on\n");
}
if(carmen_strcasecmp(awbmode, "Custom") == 0) {
if (carmen_param_get_int("awbred", &awbred,NULL) < 0)
carmen_die("camera_awbred must be specified or carmen_awbmode\n"
"cannot be set to custom.\n");
if (carmen_param_get_int("awbblue", &awbblue,NULL) < 0)
carmen_die("camera_awbblue must be specified or carmen_awbmode\n"
"cannot be set to custom.\n");
}
}
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 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]);
}
int main(int argc, char **argv)
{
char *action;
if (argc < 2)
main_usage(argv[0]);
action = argv[1];
if (carmen_strcasecmp(action, "-h") == 0 ||
carmen_strcasecmp(action, "--help") == 0)
main_usage(argv[0]);
if (carmen_strcasecmp(action, "help") == 0) {
help(argc, argv);
return 0;
}
if (carmen_strcasecmp(action, "toppm") == 0)
toppm(argc, argv);
#ifndef NO_GRAPHICS
else if (carmen_strcasecmp(action, "tomap") == 0)
tomap(argc, argv);
#endif
else if (carmen_strcasecmp(action, "rotate") == 0)
rotate(argc, argv);
else if (carmen_strcasecmp(action, "minimize") == 0)
minimize(argc, argv);
else if (carmen_strcasecmp(action, "add_place") == 0)
add_place(argc, argv);
else if (carmen_strcasecmp(action, "add_offset") == 0)
add_offset(argc, argv);
else if (carmen_strcasecmp(action, "strip") == 0)
strip(argc, argv);
else if (carmen_strcasecmp(action, "info") == 0)
info(argc, argv);
else {
carmen_warn("\nUnrecognized action %s\n", argv[1]);
main_usage(argv[0]);
}
return 0;
}
static void setup_camera(void)
{
struct video_picture svp;
struct pwc_whitebalance pw;
struct video_window svw;
int tmp = 1;
if(ioctl(camera_fd, VIDIOCPWCSCQUAL, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
if(useagc) {
tmp = -1;
if(ioctl(camera_fd, VIDIOCPWCSAGC, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
} else {
if(ioctl(camera_fd, VIDIOCPWCSAGC, &gain) != 0)
carmen_die_syserror("Could not set camera parameters");
}
if(useautoshutter) {
tmp = -1;
if(ioctl(camera_fd, VIDIOCPWCSSHUTTER, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
} else {
if(ioctl(camera_fd, VIDIOCPWCSSHUTTER, &shutterlength) != 0)
carmen_die_syserror("Could not set camera parameters");
}
if(ioctl(camera_fd, VIDIOCPWCGAWB, &pw) != 0)
carmen_die_syserror("Could not set camera parameters");
if (carmen_strcasecmp(awbmode, "Auto") == 0) {
pw.mode = PWC_WB_AUTO;
} else if(carmen_strcasecmp(awbmode, "Custom") == 0) {
pw.mode = PWC_WB_MANUAL;
pw.manual_red = awbred;
pw.manual_blue = awbblue;
} else if(carmen_strcasecmp(awbmode, "Indoor") == 0) {
pw.mode = PWC_WB_INDOOR;
} else if(carmen_strcasecmp(awbmode, "Outdoor") == 0) {
pw.mode = PWC_WB_OUTDOOR;
} else if(carmen_strcasecmp(awbmode, "Fluorescent") == 0) {
pw.mode = PWC_WB_FL;
} else {
carmen_warn("Unknown white balance mode %s!\n", awbmode);
pw.mode = PWC_WB_AUTO;
}
if(ioctl(camera_fd, VIDIOCPWCSAWB, &pw) != 0)
carmen_die_syserror("Could not set camera parameters");
// [fix] do something with the led?
if(useautosharpen) {
tmp = -1;
if(ioctl(camera_fd, VIDIOCPWCSCONTOUR, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
} else {
if(ioctl(camera_fd, VIDIOCPWCSCONTOUR, &sharpenstrength) != 0)
carmen_die_syserror("Could not set camera parameters");
}
if(backlightcompensation) {
tmp = 1;
if(ioctl(camera_fd, VIDIOCPWCSBACKLIGHT, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
} else {
tmp = 0;
if(ioctl(camera_fd, VIDIOCPWCSBACKLIGHT, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
}
if(antiflicker) {
tmp = 1;
if(ioctl(camera_fd, VIDIOCPWCSFLICKER, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
} else {
tmp = 0;
if(ioctl(camera_fd, VIDIOCPWCSFLICKER, &tmp) != 0)
carmen_die_syserror("Could not set camera parameters");
}
if(ioctl(camera_fd, VIDIOCPWCSDYNNOISE, &denoisestrength) != 0)
carmen_die_syserror("Could not set camera parameters");
if(ioctl(camera_fd, VIDIOCGPICT, &svp) != 0)
carmen_die_syserror("Could not set camera parameters");
if(brightness != -1) svp.brightness = brightness;
if(hue != -1) svp.hue = hue;
if(saturation != -1) svp.colour = saturation;
if(contrast != -1) svp.contrast = contrast;
if(camera_gamma != -1) svp.whiteness = camera_gamma;
if(dospecial) svp.palette = VIDEO_PALETTE_YUV420P;
if(ioctl(camera_fd, VIDIOCSPICT, &svp) != 0)
carmen_die_syserror("Could not set camera parameters");
if(ioctl(camera_fd, VIDIOCGWIN, &svw) != 0)
carmen_die_syserror("Could not set camera parameters");
if(dospecial) svw.width = image_width;
if(dospecial) svw.height = image_height;
svw.flags &= ~PWC_FPS_FRMASK;
svw.flags |= (fps << PWC_FPS_SHIFT);
if(ioctl(camera_fd, VIDIOCSWIN, &svw) != 0)
carmen_die_syserror("Could not set camera parameters");
}
