예제 #1
0
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);
}
예제 #2
0
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);
}
예제 #3
0
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;
}
예제 #4
0
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;
}
예제 #5
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;
}
예제 #6
0
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); 
}
예제 #7
0
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;
}
예제 #8
0
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));
}
예제 #9
0
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));
}
예제 #10
0
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);
}
예제 #11
0
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);
}
예제 #12
0
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;
}
예제 #13
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");
}