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);
}
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);
}
carmen_camera_image_t *carmen_camera_start(int argc, char **argv)
{
carmen_camera_image_t *image;
read_parameters(argc, argv);
carmen_warn("Opening camera\n");
camera_fd = open(camera_dev, O_RDONLY | O_NOCTTY);
if (camera_fd < 0)
carmen_die_syserror("Could not open %s as camera device", camera_dev);
check_camera_type();
setup_camera();
image = (carmen_camera_image_t *)calloc(1, sizeof(carmen_camera_image_t));
image->width = image_width;
image->height = image_height;
image->bytes_per_pixel = 3;
image->image_size = image->width*image->height*image->bytes_per_pixel;
image->is_new = 0;
image->timestamp = 0;
image->image = (char *)calloc(image->image_size, sizeof(char));
carmen_test_alloc(image->image);
memset(image->image, 0, image->image_size*sizeof(char));
return image;
}
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;
}
carmen_map_p
read_beesoft_map(char *filename)
{
FILE *in_fp;
carmen_map_p map;
int x_index, y_index;
double *map_ptr;
char buf[1024];
map = (carmen_map_p)calloc(1, sizeof(carmen_map_t));
carmen_test_alloc(map);
in_fp = fopen(filename, "r");
if (in_fp == NULL)
carmen_die_syserror("Couldn't open %s for reading", filename);
while (fgets(buf, 1024, in_fp) != NULL &&
strncmp("global_map[0]", buf, 13) != 0)
{
if (strncmp("robot_specifications->resolution", buf, 32) == 0)
{
sscanf(buf+32, "%lf", &(map->config.resolution));
map->config.resolution /= 100.0;
}
}
sscanf(buf, "%*s %d %d", &(map->config.y_size), &(map->config.x_size));
map->complete_map = (double *)
calloc(map->config.x_size*map->config.y_size, sizeof(double));
carmen_test_alloc(map->complete_map);
map->map = (double **)calloc(map->config.x_size, sizeof(double *));
carmen_test_alloc(map->map);
for (x_index = 0; x_index < map->config.x_size; x_index++)
map->map[x_index] = map->complete_map+x_index*map->config.y_size;
map_ptr = map->complete_map;
for (x_index = 0; x_index < map->config.x_size; x_index++)
for (y_index = 0; y_index < map->config.y_size; y_index++)
{
fscanf(in_fp, "%f", map_ptr);
if (*map_ptr >= 0)
*map_ptr = 1- *map_ptr;
map_ptr++;
}
fclose(in_fp);
return map;
}
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);
}
carmen_map_p carmen_map_imagefile_to_map(char *filename, double resolution)
{
carmen_map_p map;
GdkPixbuf* buf;
GError *error;
error = NULL;
buf = gdk_pixbuf_new_from_file(filename, &error);
if (buf == NULL)
carmen_die_syserror("Couldn't open %s for reading", filename);
map = carmen_pixbuf_to_map(buf, resolution);
g_object_unref(buf);
return map;
}
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));
}
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[])
{
struct stat buf;
double x, y, theta, tv, rv;
double left_disp, right_disp, delta_time;
int fd;
int unused_return_value;
if (argc != 2 || carmen_strncasecmp(argv[1], "-h", 2) == 0 ||
carmen_strncasecmp(argv[1], "--h", 3) == 0) {
if (argc > 2)
carmen_warn("Too many arguments.\n\n");
else if (argc < 2)
carmen_warn("Not enough arguments.\n\n");
carmen_die("Usage: %s <serial dev name>\n", argv[0]);
}
// Does the device exist?
carmen_warn("Device exists test: ");
if (stat(argv[1], &buf) == 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device exists test");
}
// Can we open the device?
carmen_warn("Device open test: ");
fd = open(argv[1], O_RDWR | O_SYNC | O_NOCTTY, 0);
if (fd >= 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device open test");
}
// close(fd);
// Can we talk to the orc board properly?
carmen_warn("Orc recognized test: ");
if (carmen_base_direct_initialize_robot("orc", argv[1]) == 0)
carmen_warn("OK\n");
else {
carmen_die("FAILED\n");
}
// Can we move the wheels?
carmen_warn("Orc drive test: %sMAKE SURE THE ROBOT IS ON BLOCKS%s.\n",
carmen_red_code, carmen_normal_code);
carmen_warn("Hit return to start the orc drive test....");
unused_return_value = scanf("%*c");
if (1) {
// Move left wheel
carmen_warn("Left wheel forwards test: ");
carmen_base_command_velocity(1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. The motors need "
"to be swapped.\n");
if (left_disp < -1e-3)
carmen_die("FAILED\n Left encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Left wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. Very odd.\n");
if (left_disp > 1e-3)
carmen_die("FAILED\n Left encoder moved forwards. Very odd.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
// Move right wheel
carmen_warn("Right wheel forwards test: ");
carmen_base_command_velocity(1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. This is very odd, since it"
"also moved with\nthe other wheel.\n");
if (right_disp < -1e-3)
carmen_die("FAILED\nRight encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Right wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. Very odd.\n");
if (right_disp > 1e-3)
carmen_die("FAILED\nRight encoder moved forwards. Very odd.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
}
carmen_warn("OK\n");
carmen_base_direct_reset();
carmen_warn("Orc drive test: ");
if (carmen_base_direct_set_velocity(1.0, 0.0) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Let some encoder data build up
carmen_warn("Sleep\n");
sleep(1);
carmen_base_direct_set_velocity(0.0, 0.0);
// Are we getting encoder data?
carmen_warn("Orc encoder test: ");
if (carmen_base_direct_get_integrated_state(&x, &y, &theta, &tv, &rv) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Is encoder data valid?
carmen_warn("Orc encoder data validity test: ");
if (fabs(x) > 0 && fabs(y) < 1e-2 &&
fabs(carmen_radians_to_degrees(theta)) < 10)
carmen_warn("OK (%.2f)\n", x);
else
carmen_die("FAILED (%.2f %.2f %.2f)\n", x, y,
carmen_radians_to_degrees(theta));
carmen_base_direct_shutdown_robot();
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");
}
