int 
main(int argc, char **argv)
{
	FILE *flist = fopen(argv[2], "r");
	carmen_FILE *fin;
	carmen_FILE *fout;

	char fname_in[2000];
	char fname_out[2000];
	char map_name[2000];

	if (argc != 5)
	{
		printf("Usage:\n convert_maps_from_float_to_double <number_of_maps> <file_with_map_list.txt> <path_of_float_maps> <path_of_double_maps>\n");
		exit(0);
	}
	
	int number_of_maps = atoi(argv[1]);

	for (int i = 0; i < number_of_maps; i++)
	{
		fscanf(flist, "%s", map_name);
		printf("%s\n", map_name);
		sprintf(fname_in, "%s/%s", argv[3], map_name);
		sprintf(fname_out, "%s/%s", argv[4], map_name);

		fin = carmen_fopen(fname_in, "r");
		fout = carmen_fopen(fname_out, "w");

		carmen_map_read_gridmap(fin, fout);

		carmen_fclose(fin);
		carmen_fclose(fout);
	}
}
static void carmen_map_read_gridmap(carmen_FILE *fin, carmen_FILE *fout)
{
  int size_x, size_y;
  float resolution;
  float *complete_map = NULL;
  carmen_map_t map;

  int chunk_type, chunk_size;
  char chunk_description[12];

  if(carmen_map_advance_to_chunk(fin, CARMEN_MAP_GRIDMAP_CHUNK) < 0) {
      fprintf(stderr, "Error: Could not find a gridmap chunk.\n");
      fprintf(stderr, "       This file is probably not a map file.\n");
      carmen_fclose(fin);
    }

  chunk_type = carmen_fgetc(fin);
  carmen_fread(&chunk_size, sizeof(int), 1, fin);
  carmen_fread(chunk_description, 10, 1, fin);

  chunk_description[10] = '\0';

  if(CARMEN_MAP_CHUNK_IS_NAMED(chunk_type)) {
    if(read_string(NULL, -1, fin) < 0) {
      carmen_warn("Error: Unexpected EOF.\n");
      carmen_fclose(fin);
    }
  }

  carmen_fread(&size_x, sizeof(int), 1, fin);
  carmen_fread(&size_y, sizeof(int), 1, fin);
  carmen_fread(&resolution, sizeof(float), 1, fin);

  printf("%d %d %lf\n", size_x, size_y, resolution);

  carmen_grid_mapping_create_new_map(&map, size_x, size_y, resolution);
  complete_map = (float *)calloc(map.config.x_size * map.config.y_size, sizeof(float));

  carmen_fread(complete_map, sizeof(float) * size_x * size_y, 1, fin);

  for (int i = 0; i < map.config.x_size * map.config.y_size; i++)
  {
	  map.complete_map[i] = complete_map[i];
  }


  carmen_map_write_all(fout, map.map,
  			map.config.x_size,
  			map.config.y_size,
  			map.config.resolution,
  			(char *)"",	(char *)"", (char *)"", (char *)"Generated by LCAD-UFES",
  			NULL, 0, NULL, 0, NULL, 0);
  free(complete_map);
  free(map.complete_map);
  free(map.map);
}
Esempio n. 3
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); 
}
Esempio n. 4
0
void shutdown_module(int sig)
{
  if(sig == SIGINT) {
    carmen_fclose(outfile);
    carmen_ipc_disconnect();
    fprintf(stderr, "\nDisconnecting.\n");
    exit(0);
  }
}
Esempio n. 5
0
bool LogFile::load(const char* filename, bool verbose) {
  char line[100000];

  carmen_FILE *logfile = NULL;
  carmen_logfile_index_p logfile_index = NULL;

  logfile = carmen_fopen(filename, "r");
  if(logfile == NULL) {
    if (verbose)
      carmen_warn("Error: could not open file %s for reading.\n", filename);
    return false;
  }

  /* index the logfile */
  logfile_index = carmen_logfile_index_messages(logfile);

  for(int i = 0; i < logfile_index->num_messages; i++) {
    
    /* read i-th line */
    carmen_logfile_read_line(logfile_index, logfile, i, 100000, line);

    /* create messages */
    if(strncmp(line, "ODOM ", 5) == 0) {
      push_back(new OdometryMessage(line));
    }
    else if(strncmp(line, "RAWLASER", 8) == 0) {
      push_back(new LaserMessage(line));
    }
    else if(strncmp(line, "ROBOTLASER", 10) == 0) {
      push_back(new RobotLaserMessage(line));
    }
    else if(strncmp(line, "FLASER ", 7) == 0) {
      push_back(new RobotLaserMessage(line));
    }
    else if(strncmp(line, "RLASER ", 7) == 0) {
      push_back(new RobotLaserMessage(line));
    }
    else if(strncmp(line, "TRUEPOS ", 8) == 0) {
      push_back(new TrueposMessage(line));
    }
    else if(strncmp(line, "IMU ", 4) == 0) {
      push_back(new IMUMessage(line));
    }
    else if(strncmp(line, "SCANMARK ", 9) == 0) {
      push_back(new ScanmarkMessage(line));
    }
    else if(strncmp(line, "POSITIONLASER ", 14) == 0) {
      push_back(new LaserposMessage(line));
    }
    else if(strlen(line) > 1) {
      push_back(new UnknownMessage(line));
    }
  }
  carmen_fclose(logfile);  
  return true;
}
void
save_local_map()
{
	carmen_FILE *fp;
	fp = carmen_fopen("local.map", "w");
	carmen_map_write_all(fp, local_map.map,
				local_map.config.x_size,
				local_map.config.y_size,
				local_map.config.resolution,
				(char *) "", (char *) "", (char *) "", (char *) "Generated by big_map",
				NULL, 0, NULL, 0, NULL, 0);
	carmen_fclose(fp);
}
Esempio n. 7
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);
}
Esempio n. 8
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;
}
Esempio n. 9
0
int main(int argc, char **argv)
{
  char filename[1024];
  char key;

  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);	

  if(argc < 2) 
    carmen_die("usage: %s <logfile>\n", argv[0]);

  sprintf(filename, "%s", argv[1]);

  outfile = carmen_fopen(filename, "r");
  if (outfile != NULL) {
    fprintf(stderr, "Overwrite %s? ", filename);
    scanf("%c", &key);
    if (toupper(key) != 'Y')
      exit(-1);
    carmen_fclose(outfile);
  }
  outfile = carmen_fopen(filename, "w");
  if(outfile == NULL)
    carmen_die("Error: Could not open file %s for writing.\n", filename);
  carmen_logwrite_write_header(outfile);

  carmen_robot_subscribe_frontlaser_message(NULL, 
					    (carmen_handler_t)robot_frontlaser_handler, 
					    CARMEN_SUBSCRIBE_ALL);
  localize_msg = (carmen_localize_globalpos_message *)
    calloc(1, sizeof(carmen_localize_globalpos_message));
  carmen_test_alloc(localize_msg);
  carmen_localize_subscribe_globalpos_message(localize_msg, NULL, CARMEN_SUBSCRIBE_ALL);

  logger_starttime = carmen_get_time();


  signal(SIGINT, shutdown_module);
  carmen_ipc_dispatch();
  return 0;
}
Esempio n. 10
0
bool LogFile::save(const char* filename, bool verbose) const {
  
  carmen_FILE *logfile = NULL;
  
  logfile = carmen_fopen(filename, "w");
  if(logfile == NULL) {
    if (verbose)
      carmen_warn("Error: could not open file %s for writing.\n", filename);
    return false;
  }
  
  double logger_starttime = -1.0;
  
  for (Collection::const_iterator it = begin();
       it != end(); ++it) {
    if (logger_starttime < 0)
      logger_starttime = (*it)->getTimestamp();
    (*it)->save(logfile, (*it)->getTimestamp() - logger_starttime) ;
  }
  carmen_fclose(logfile);
  return true;
}
Esempio n. 11
0
int main(int argc, char **argv)
{
  char filename[1024];
  char key;
  int num_scanned;

  /* initialize connection to IPC network */
  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);	

  /* open logfile, check if file overwrites something */
  if(argc < 2) 
    carmen_die("usage: %s <logfile>\n", argv[0]);
  sprintf(filename, "%s", argv[1]);

  outfile = carmen_fopen(filename, "r");
  if (outfile != NULL) {
    fprintf(stderr, "Overwrite %s? ", filename);
    num_scanned = scanf("%c", &key);
    if (toupper(key) != 'Y')
      exit(-1);
    carmen_fclose(outfile);
  }
  outfile = carmen_fopen(filename, "w");
  if(outfile == NULL)
    carmen_die("Error: Could not open file %s for writing.\n", filename);
  carmen_logwrite_write_header(outfile);


  get_logger_params(argc, argv);

  if  ( !(log_odometry && log_laser && log_robot_laser ) )
    carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
  


  if (log_params)
    get_all_params();

  register_ipc_messages();


  if (log_odometry) 
    carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
					   base_odometry_handler, 
					   CARMEN_SUBSCRIBE_ALL);

  if (log_sonars) 
    carmen_base_subscribe_sonar_message(NULL, (carmen_handler_t)
					   base_sonar_handler, 
					   CARMEN_SUBSCRIBE_ALL);
  if (log_bumpers) 
    carmen_base_subscribe_bumper_message(NULL, (carmen_handler_t)
					   base_bumper_handler, 
					   CARMEN_SUBSCRIBE_ALL);

  if (log_arm) 
    carmen_arm_subscribe_state_message(NULL, (carmen_handler_t)
				       arm_state_handler, 
				       CARMEN_SUBSCRIBE_ALL);

  if (log_pantilt) {
    carmen_pantilt_subscribe_scanmark_message (NULL, (carmen_handler_t)
					       pantilt_scanmark_handler, 
					       CARMEN_SUBSCRIBE_ALL);
    
    carmen_pantilt_subscribe_laserpos_message (NULL, (carmen_handler_t)
					       pantilt_laserpos_handler, 
					       CARMEN_SUBSCRIBE_ALL);
  }

  if (log_robot_laser) {
    carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
					      robot_frontlaser_handler, 
					      CARMEN_SUBSCRIBE_ALL);
    carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
					     robot_rearlaser_handler, 
					     CARMEN_SUBSCRIBE_ALL);
  }

  if (log_laser) {
    carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
					  laser_laser1_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
					  laser_laser2_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
					  laser_laser3_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
					  laser_laser4_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
					  laser_laser5_handler, 
					  CARMEN_SUBSCRIBE_ALL);
  }

  if (log_localize) {
    carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
						localize_handler,
						CARMEN_SUBSCRIBE_ALL);
  }

  if (log_simulator) {

    carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)  
					       carmen_simulator_truepos_handler,
					       CARMEN_SUBSCRIBE_ALL);
  }

  if (log_imu) {

    carmen_imu_subscribe_imu_message(NULL, (carmen_handler_t)  
				     imu_handler,
				     CARMEN_SUBSCRIBE_ALL);
  }
  
  
  if (log_gps) {
    carmen_gps_subscribe_nmea_message( NULL,
				       (carmen_handler_t) ipc_gps_gpgga_handler,
				       CARMEN_SUBSCRIBE_ALL );
    
    carmen_gps_subscribe_nmea_rmc_message( NULL,
					   (carmen_handler_t) ipc_gps_gprmc_handler,
					   CARMEN_SUBSCRIBE_ALL );
  }
  
  if (log_motioncmds) {
  	 carmen_robot_subscribe_vector_move_message( NULL,
  	 					 (carmen_handler_t) robot_vector_move_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );
  	 carmen_robot_subscribe_follow_trajectory_message( NULL,
  	 					 (carmen_handler_t) robot_follow_trajectory_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );
	 carmen_robot_subscribe_velocity_message( NULL,
	 					 (carmen_handler_t) robot_velocity_handler,
	 					 CARMEN_SUBSCRIBE_ALL );		
	 carmen_base_subscribe_velocity_message( NULL,
	 					 (carmen_handler_t) base_velocity_handler,
	 					 CARMEN_SUBSCRIBE_ALL );							 			 
  }

  carmen_logger_subscribe_comment_message( NULL,
  	 					 (carmen_handler_t) logger_comment_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );

  signal(SIGINT, shutdown_module);
  logger_starttime = carmen_get_time();
  carmen_ipc_dispatch();
  return 0;
}
Esempio n. 12
0
int
main( int argc, char *argv[] )
{ 
  carmen_point_t                  pos, corrpos, odo;
  double                          time;
  int                             i;
  int                             scancnt;
  carmen_move_t                   pos_move = {0.0, 0.0, 0.0};
  carmen_move_t                   corr_move = {0.0, 0.0, 0.0};
  carmen_point_t                  old_pos = {0.0, 0.0, 0.0};
  carmen_point_t                  old_corrpos = {0.0, 0.0, 0.0};
  int which_laser = 0;

  carmen_FILE stdout_carmen;
  stdout_carmen.compressed = 0;  
  stdout_carmen.fp = stdout;

  if (argc < 2) {
    print_usage();
    exit(1);
  }

  scancnt=0;
  for (i=1; i < argc-1; i++) {
    if (!strcmp(argv[i], "-f")  || !strcmp(argv[i], "--flaser"))
      which_laser = 1;
    if (!strcmp(argv[i], "-r")  || !strcmp(argv[i], "--robotlaser"))
      which_laser = 2;
  }

  carmen_ipc_initialize(argc, argv);
  vascocore_init(argc, argv);

  /*   file handler */
  carmen_FILE *logfile = NULL;

  /*   log file index structure */
  carmen_logfile_index_p logfile_index = NULL;

  /*  open logfile for reading */
  logfile = carmen_fopen(argv[argc-1], "r");
  if(logfile == NULL)
    carmen_die("Error: could not open file %s for reading.\n", argv[1]);

  /* create index structure */
  logfile_index = carmen_logfile_index_messages(logfile);
  
  /*   buffer for reading the lines */
  const int max_line_length=9999;
  char line[max_line_length+1];
  
  /*   used to read the messages. erase stucture before! */
  carmen_robot_laser_message l;
  carmen_erase_structure(&l, sizeof(carmen_robot_laser_message) );

  /*   iterate over all entries in the logfile */
  while (!carmen_logfile_eof(logfile_index)) {

    int correct_msg = 1;
    /*     read the line from the file */
    carmen_logfile_read_next_line(logfile_index, 
				  logfile,
				  max_line_length, 
				  line);
    
    /*     what kind of message it this? */
    if (which_laser != 1 && strncmp(line, "ROBOTLASER1 ", 12) == 0) {
      /*  convert the string using the corresponding read function */
      char* next = carmen_string_to_robot_laser_message(line, &l);
      time = atof( strtok( next, " ") );

    }
    else if (which_laser != 2 && strncmp(line, "FLASER ", 7) == 0) {
      /*  convert the string using the corresponding read function */
      char* next = carmen_string_to_robot_laser_message_orig(line, &l);
      time = atof( strtok( next, " ") );
    }
    else {
      correct_msg = 0;
      fputs(line, stdout);
    }

    if (correct_msg) {
      odo = l.robot_pose;
      pos = l.laser_pose;

      /* using the new scan match function that takes robot laser messages...
      carmen_laser_laser_message scan;
      carmen_erase_structure(&scan,  sizeof(carmen_laser_laser_message) );
      scan.timestamp      = l.timestamp;
      scan.config         = l.config;
      scan.num_readings   = l.num_readings;
      scan.num_remissions = l.num_remissions;
      scan.range          = l.range;
      scan.remission      = l.remission;
      scan.host           = l.host;

      corrpos = vascocore_scan_match( scan, pos );*/
      corrpos = vascocore_scan_match_robot( l );

      scancnt++;
      corr_move = carmen_move_between_points( old_corrpos, corrpos );
      pos_move = carmen_move_between_points( old_pos, pos );

      fprintf( stderr, "expected movement  %.6f m %.6f m %.6f degree\n",
	       corr_move.forward, corr_move.sideward,
	       rad2deg(corr_move.rotation) );
      fprintf( stderr, "corrected movement %.6f m %.6f m %.6f degree\n\n",
	       pos_move.forward, pos_move.sideward,
	       rad2deg(pos_move.rotation) );

      l.laser_pose = corrpos;

      carmen_logwrite_write_robot_laser(&l,1,&stdout_carmen, time);
      fflush(stdout);
   
      old_pos     = pos;
      old_corrpos = corrpos;
    }
    
  }
  
  /* close the file */
  carmen_fclose(logfile);

  fprintf(stderr, "\ndone, corrected %d messages!\n", scancnt);

  /* free index structure */
  carmen_logfile_free_index(&logfile_index);
  
  return(0);
  
}
int main(int argc, char **argv)
{
  FILE *fp;
  carmen_FILE *outfile;
  int m, d, y, h, min, temp1, temp2, i;
  float s;
//  int type_id;
  double timestamp, first_timestamp = 0;
  double last_front_laser_timestamp, last_odometry_timestamp;
  int first = 1;
  char key;

  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);	

  if(argc < 3) {
    fprintf(stderr, "Error: wrong number of arguments.\n");
    fprintf(stderr, "Usage: %s laserint-logname logname\n", argv[0]);
    exit(1);
  }
  fp = fopen(argv[1], "r");
  if(fp == NULL) {
    fprintf(stderr, "Error: could not open file %s for reading.\n", argv[1]);
    exit(1);
  }
	outfile = carmen_fopen(argv[2], "r");
	if (outfile != NULL) {
		fprintf(stderr, "Overwrite %s? ", argv[2]);
		scanf("%c", &key);
		if (toupper(key) != 'Y')
			exit(-1);
		carmen_fclose(outfile);
	}

  outfile = carmen_fopen(argv[2], "w");
  if(outfile == NULL) {
    fprintf(stderr, "Error: could not open file %s for writing.\n", argv[2]);
    exit(1);
  }

	front_laser.num_readings = 180;
	front_laser.range = (float *)calloc(180, sizeof(float));
	carmen_test_alloc(front_laser.range);

	front_laser.host = carmen_get_host();
	odometry.host = carmen_get_host();

	carmen_logwrite_write_header(outfile);
	carmen_logwrite_write_robot_name("beesoft", outfile);
  get_all_params(outfile);

  while(!feof(fp)) {
    fgets(line, LINE_SIZE, fp);
    if(strncmp(line, "@SENS", 5) == 0) {

      fgets(line2, 7, fp);

      if(strncmp(line2, "#LASER", 6) == 0) {
				last_front_laser_timestamp = front_laser_timestamp;
	
				sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s);
				timestamp = h * 3600.0 + min * 60.0 + s;
				fscanf(fp, " %d %d: ", &temp1, &temp2);
				for(i = 0; i < 180; i++) {
					fscanf(fp, "%d", &temp1);
					front_laser.range[i] = temp1/100.0;
				}
				front_laser.laser_pose.x = odometry.x;
				front_laser.laser_pose.y = odometry.y;
				front_laser.laser_pose.theta = 
				  odometry.theta;

 				front_laser.robot_pose.x = odometry.x;
				front_laser.robot_pose.y = odometry.y;
				front_laser.robot_pose.theta = 
				  odometry.theta;

				if(first) {
					first_timestamp = timestamp;
					first = 0;
				}
				front_laser_timestamp = timestamp - first_timestamp;

				if(current_front_laser > 0 && 
					 front_laser_timestamp < last_front_laser_timestamp)
					front_laser_timestamp = last_front_laser_timestamp;

//				type_id = ROBOT_FRONTLASER_ID;

				front_laser.timestamp = front_laser_timestamp;
				
				carmen_logwrite_write_robot_laser(&front_laser, 1, outfile, front_laser_timestamp);
	  
				current_front_laser++;
      }
      else if(strncmp(line2, "#ROBOT", 6) == 0) {
				last_odometry_timestamp = odometry_timestamp;
	
				sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s);
				timestamp = h * 3600.0 + min * 60.0 + s;
				fscanf(fp, " %lf %lf %lf", &odometry.x, &odometry.y, 
							 &odometry.theta);
				odometry.x /= 100.0;
				odometry.y /= 100.0;
				odometry.theta = carmen_normalize_theta(carmen_degrees_to_radians(odometry.theta));
	
				if(first) {
					first_timestamp = timestamp;
					first = 0;
				}
				odometry_timestamp = timestamp - first_timestamp;

				if(current_odometry > 0 &&
					 odometry_timestamp < last_odometry_timestamp)
					odometry_timestamp = last_odometry_timestamp;

//				type_id = ODOM_ID;
 				carmen_logwrite_write_odometry(&odometry, outfile, odometry_timestamp); 

				current_odometry++;
      }
      fgets(line2, LINE_SIZE, fp);
    }
  }

  fclose(fp);
  carmen_fclose(outfile);
  return 0;
}
Esempio n. 14
0
//
// This calls the procedures in the other files which do all the real work. 
// If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make
// certain to set LOW_DURATION in low.h to some incredibly high number.
//
void *Slam(void * /*arg unused */)
{
  TPath *path, *trashPath;
  TSenseLog *obs, *trashObs;
  double oldmC_D = 0;
  double oldmC_T = 0;
  double oldvC_D = 0;
  double oldvC_T = 0;
  double oldmD_D = 0;
  double oldmD_T = 0;
  double oldvD_D = 0;
  double oldvD_T = 0;
  double oldmT_D = 0;
  double oldmT_T = 0;
  double oldvT_D = 0;
  double oldvT_T = 0;

  outFile = fopen("motionModel.txt", "w");
  fprintf(outFile, "This file lists the motion model parameters in the following order : \n");
  fprintf(outFile, "    - mean value from observed translation\n");
  fprintf(outFile, "    - mean value from observed rotation\n");
  fprintf(outFile, "    - variance contributed from observed translation\n");
  fprintf(outFile, "    - variance contributed from observed rotation\n");
  fprintf(outFile, "\n");
  fprintf(outFile, "------------Intermediate Models------------------\n");
  fprintf(outFile, "   m_dist m_turn   v_dist v_turn\n");
  fprintf(outFile, "C: %.4f %.4f   %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
  fprintf(outFile, "D: %.4f %.4f   %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
  fprintf(outFile, "T: %.4f %.4f   %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
  fprintf(outFile, "\n");
  fclose(outFile);

  while ((fabs(oldmC_D-meanC_D) > 0.01)||(fabs(oldmC_T-meanC_T) > 0.03)||(fabs(oldvC_D-varC_D) > 0.01)||(fabs(oldvC_T-varC_T) > 0.1) || 
	 (fabs(oldmD_D-meanD_D) > 0.01)||(fabs(oldmD_T-meanD_T) > 0.03)||(fabs(oldvD_D-varD_D) > 0.01)||(fabs(oldvD_T-varD_T) > 0.1) || 
	 (fabs(oldmT_D-meanT_D) > 0.03)||(fabs(oldmT_T-meanT_T) > 0.01)||(fabs(oldvT_D-varT_D) > 0.03)||(fabs(oldvT_T-varT_T) > 0.01)) {

    readFile = carmen_fopen(PLAYBACK, "r");
    if(readFile == NULL)
      carmen_die("Error: could not open file %s for reading.\n", PLAYBACK);
    logfile_index = carmen_logfile_index_messages(readFile);

    InitLowSlam();
    LowSlam(&path, &obs);
    carmen_fclose(readFile);

    oldmC_D = meanC_D;    
    oldmC_T = meanC_T;
    oldvC_D = varC_D;    
    oldvC_T = varC_T;
    oldmD_D = meanD_D;    
    oldmD_T = meanD_T;
    oldvD_D = varD_D;    
    oldvD_T = varD_T;
    oldmT_D = meanT_D;    
    oldmT_T = meanT_T;
    oldvT_D = varT_D;    
    oldvT_T = varT_T;

    outFile = fopen("motionModel.txt", "a");
    Learn(path);
    fclose(outFile);

    // Get rid of the path and log of observations
    while (path != NULL) {
      trashPath = path;
      path = path->next;
      free(trashPath);
    }
    while (obs != NULL) {
      trashObs = obs;
      obs = obs->next;
      free(trashObs);
    }
  }


  outFile = fopen("motionModel.txt", "a");
  fprintf(outFile, "\n");
  fprintf(outFile, "------------Final Model------------------\n");
  fprintf(outFile, "   m_dist m_turn   v_dist v_turn\n");
  fprintf(outFile, "C: %.4f %.4f   %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
  fprintf(outFile, "D: %.4f %.4f   %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
  fprintf(outFile, "T: %.4f %.4f   %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);

  fprintf(outFile, "\n");
  fprintf(outFile, "#define meanC_D %.4f\n#define meanC_T %.4f\n#define varC_D %.4f\n#define varC_T %.4f\n\n", meanC_D, meanC_T, varC_D, varC_T);
  fprintf(outFile, "#define meanD_D %.4f\n#define meanD_T %.4f\n#define varD_D %.4f\n#define varD_T %.4f\n\n", meanD_D, meanD_T, varD_D, varD_T);
  fprintf(outFile, "#define meanT_D %.4f\n#define meanT_T %.4f\n#define varT_D %.4f\n#define varT_T %.4f\n\n", meanT_D, meanT_T, varT_D, varT_T);
  fclose(outFile);

  CloseLowSlam();
  return NULL;
}
Esempio n. 15
0
glv_object_p glv_object_read(char *filename) {
  carmen_FILE *fp;
  glv_color_t current_color;
  char buffer[10000];
  long int nread, log_bytes = 0;
  int buffer_pos, buffer_length, offset = 0, n;
  glv_object_p obj;
  float *fmessage;
  int done_reading = 0;
  int i, line_count = 0;
  float min_x, max_x, min_y, max_y, min_z, max_z;

  if(strncmp(filename + strlen(filename) - 4, ".glv", 4) &&
    strncmp(filename + strlen(filename) - 7, ".glv.gz", 7) &&
    strncmp(filename + strlen(filename) - 5, ".pmap", 5) &&
    strncmp(filename + strlen(filename) - 8, ".pmap.gz", 8))
    carmen_die("Error: file name must end in .glv, .glv.gz, .pmap, or .pmap.gz\n");
  fp = carmen_fopen(filename, "r");

  /* compute total number of bytes in logfile */
  do {
    nread = carmen_fread(buffer, 1, 10000, fp);
    log_bytes += nread;
  } while(nread > 0);
  carmen_fseek(fp, 0L, SEEK_SET);

  current_color.r = 255;
  current_color.g = 255;
  current_color.b = 255;

  obj = glv_object_init();

  buffer_pos = 0;
  buffer_length = carmen_fread(buffer, 1, 10000, fp);

  while(!done_reading || buffer_length > buffer_pos) {
    line_count++;
    if(line_count % 100000 == 0)
      fprintf(stderr, "\rReading glv file... (%.0f%%)  ",
        (offset + buffer_pos) / (float)log_bytes * 100.0);

    if(buffer_length - buffer_pos < 50 && !done_reading) {
      memmove(buffer, buffer + buffer_pos, buffer_length - buffer_pos);
      buffer_length -= buffer_pos;
      offset += buffer_pos;
      buffer_pos = 0;
      n = carmen_fread(buffer + buffer_length,
                  1, 10000 - buffer_length - 1, fp);
      if(n == 0)
        done_reading = 1;
      else
        buffer_length += n;
    }
    else {
      if(buffer[buffer_pos] == MESSAGE_ID_COLOR) {
        current_color.r = (unsigned char)buffer[buffer_pos + 1];
        current_color.g = (unsigned char)buffer[buffer_pos + 2];
        current_color.b = (unsigned char)buffer[buffer_pos + 3];
        buffer_pos += 4;
      }
      else if(buffer[buffer_pos] == MESSAGE_ID_POINT) {
        if(obj->num_points == obj->max_points) {
          obj->max_points += 100000;
          obj->point = (glv_point_p)realloc(obj->point, obj->max_points *
                      sizeof(glv_point_t));
          carmen_test_alloc(obj->point);
        }
        fmessage = (float *)(buffer + buffer_pos + 1);
        obj->point[obj->num_points].x = fmessage[0];
        obj->point[obj->num_points].y = fmessage[1];
        obj->point[obj->num_points].z = fmessage[2];
        obj->point[obj->num_points].c = current_color;
        obj->num_points++;
        buffer_pos += 13;
      }
      else if(buffer[buffer_pos] == MESSAGE_ID_LINE) {
        if(obj->num_lines == obj->max_lines) {
          obj->max_lines += 100000;
          obj->line = (glv_line_p)realloc(obj->line, obj->max_lines *
                    sizeof(glv_line_t));
          carmen_test_alloc(obj->line);
        }
        fmessage = (float *)(buffer + buffer_pos + 1);
        obj->line[obj->num_lines].p1.x = fmessage[0];
        obj->line[obj->num_lines].p1.y = fmessage[1];
        obj->line[obj->num_lines].p1.z = fmessage[2];
        obj->line[obj->num_lines].p2.x = fmessage[3];
        obj->line[obj->num_lines].p2.y = fmessage[4];
        obj->line[obj->num_lines].p2.z = fmessage[5];
        obj->line[obj->num_lines].c = current_color;
        obj->num_lines++;
        buffer_pos += 25;
      }
      else if(buffer[buffer_pos] == MESSAGE_ID_FACE) {
        if(obj->num_faces == obj->max_faces) {
          obj->max_faces += 100000;
          obj->face = (glv_face_p)realloc(obj->face, obj->max_faces *
                    sizeof(glv_face_t));
          carmen_test_alloc(obj->face);
        }
        fmessage = (float *)(buffer + buffer_pos + 1);
        obj->face[obj->num_faces].p1.x = fmessage[0];
        obj->face[obj->num_faces].p1.y = fmessage[1];
        obj->face[obj->num_faces].p1.z = fmessage[2];
        obj->face[obj->num_faces].p2.x = fmessage[3];
        obj->face[obj->num_faces].p2.y = fmessage[4];
        obj->face[obj->num_faces].p2.z = fmessage[5];
        obj->face[obj->num_faces].p3.x = fmessage[6];
        obj->face[obj->num_faces].p3.y = fmessage[7];
        obj->face[obj->num_faces].p3.z = fmessage[8];
        obj->face[obj->num_faces].c = current_color;
        compute_normal(&obj->face[obj->num_faces]);
        obj->num_faces++;
        buffer_pos += 37;
      }
    }
  }

  min_x = 1e10;
  max_x = -1e10;
  min_y = 1e10;
  max_y = -1e10;
  min_z = 1e10;
  max_z = -1e10;
  for(i = 0; i < obj->num_points; i++)
    adjust_extrema(obj->point[i], &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
  for(i = 0; i < obj->num_lines; i++) {
    adjust_extrema(obj->line[i].p1, &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
    adjust_extrema(obj->line[i].p2, &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
  }
  for(i = 0; i < obj->num_faces; i++) {
    adjust_extrema(obj->face[i].p1, &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
    adjust_extrema(obj->face[i].p2, &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
    adjust_extrema(obj->face[i].p3, &min_x, &max_x, &min_y, &max_y,
      &min_z, &max_z);
  }
  obj->centroid.x = (min_x + max_x) / 2;
  obj->centroid.y = (min_y + max_y) / 2;
  obj->centroid.z = (min_z + max_z) / 2;
  obj->min.x = min_x;
  obj->min.y = min_y;
  obj->min.z = min_z;
  obj->max.x = max_x;
  obj->max.y = max_y;
  obj->max.z = max_z;

  carmen_fclose(fp);
  fprintf(stderr, "\rReading glv file... (100%%)   \n");
  fprintf(stderr, "%d POINTS - %d LINES - %d FACES\n", obj->num_points,
    obj->num_lines, obj->num_faces);
  return obj;
}
Esempio n. 16
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));
}
Esempio n. 17
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));
}
Esempio n. 18
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);
}
Esempio n. 19
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);
}
Esempio n. 20
0
void write_glv_output(logdata_p logdata, char *out_filename) {
  int i, j, first_scan = 1;
  transform_t t;
  transform_point_t p;
  double angle;
  double d1, d2, d3, d4;
  carmen_FILE *out_fp;

  out_fp = carmen_fopen(out_filename, "w");

  /* write path to glv file */
  write_color_glv(out_fp, 255, 255, 0);
  for (i = 1; i < logdata->num_pos; i++)
    write_line_glv(out_fp,
      logdata->pos[i-1].x, logdata->pos[i-1].y, logdata->pos[i-1].z,
      logdata->pos[i].x, logdata->pos[i].y, logdata->pos[i].z);

  /* write scan to glv file */
  for (i = 0; i < logdata->num_laser; i++) {
    if (!logdata->laser[i].ignore) {
      int num_readings = logdata->laser[i].num_readings;
      float local_x[num_readings];
      point3D_t last_scan[num_readings], current_scan[num_readings];
      char pointclass[num_readings], last_pointclass[num_readings];
  
      if(i % 100 == 0)
        fprintf(stderr, "\rProjecting points... (%.0f%%)  ",
        i/(float)logdata->num_laser*100.0);

      transform_init_identity(t);
      transform_rotate(t, logdata->laser[i].yaw, logdata->laser[i].pitch,
        logdata->laser[i].roll);
      transform_translate(t, logdata->laser[i].x, logdata->laser[i].y,
        logdata->laser[i].z);

      for (j = 0; j < num_readings; j++) {
        angle = logdata->laser[i].start_angle+
          j/(float)num_readings*logdata->laser[i].fov;

        transform_point_init(&p,
          logdata->laser[i].range[j]*cos(angle),
          logdata->laser[i].range[j]*sin(angle),
          0.0);
        transform_point(t, &p);

      	pointclass[j] = TYPE_UNKNOWN;
        if (classify_points) {
          if (logdata->laser[i].range[j] > max_range)
            pointclass[j] = TYPE_IGNORE;
          else if (p.z > 0.0)
            pointclass[j] = TYPE_OBSTACLE;
          if (pointclass[j] == TYPE_UNKNOWN)
          local_x[j] = hypot(p.x-logdata->laser[i].x, p.y-logdata->laser[i].y);
        }

        last_scan[j] = current_scan[j];
        last_pointclass[j] = pointclass[j];
        current_scan[j].x = p.x;
        current_scan[j].y = p.y;
        current_scan[j].z = p.z;
        current_scan[j].meshed = 0;
        current_scan[j].range = logdata->laser[i].range[j];
      }

      if (classify_points)
        classify_scan(num_readings, current_scan, local_x, pointclass);

      write_color_glv(out_fp, 255, 255, 255);
      for (j = 0; j < num_readings; j++) {
      	if (generate_faces && !first_scan && j > 0) {
          d1 = logdata->laser[i].range[j-1]-logdata->laser[i-1].range[j-1];
	        d2 = logdata->laser[i].range[j]-logdata->laser[i].range[j-1];
	        d3 = logdata->laser[i-1].range[j]-logdata->laser[i].range[j];
          d4 = logdata->laser[i-1].range[j-1]-logdata->laser[i-1].range[j];

          if (fabs(d1) < connect_dist && fabs(d2) < connect_dist &&
            fabs(d3) < connect_dist && fabs(d4) < connect_dist &&
            logdata->laser[i].range[j-1] < max_range &&
            logdata->laser[i-1].range[j-1] < max_range &&
            logdata->laser[i].range[j] < max_range &&
            logdata->laser[i-1].range[j] < max_range) {

            if (pointclass[j] == TYPE_OBSTACLE ||
              pointclass[j-1] == TYPE_OBSTACLE)
              write_color_glv(out_fp, 100, 100, 255);
            else
              write_color_glv(out_fp, 255, 255, 255);

            write_face_glv(out_fp,
              last_scan[j-1].x, last_scan[j-1].y, last_scan[j-1].z,
              current_scan[j-1].x, current_scan[j-1].y, current_scan[j-1].z,
              current_scan[j].x, current_scan[j].y, current_scan[j].z);
            write_face_glv(out_fp,
              last_scan[j-1].x, last_scan[j-1].y, last_scan[j-1].z,
              current_scan[j].x, current_scan[j].y, current_scan[j].z,
              last_scan[j].x, last_scan[j].y, last_scan[j].z);

            last_scan[j].meshed = 1;
            last_scan[j-1].meshed = 1;
            current_scan[j].meshed = 1;
            current_scan[j-1].meshed = 1;
          }
        }
      }

      if (generate_faces) {
        write_color_glv(out_fp, 255, 255, 255);
        for (j = 0; j < num_readings; j++) {
          if (last_pointclass[j] == TYPE_SAFE &&
            !last_scan[j].meshed && !first_scan &&
            last_scan[j].range < max_range)
            write_point_glv(out_fp, last_scan[j].x, last_scan[j].y,
				    last_scan[j].z);
        }

        write_color_glv(out_fp, 100, 100, 255);
        for (j = 0; j < num_readings; j++) {
          if (last_pointclass[j] == TYPE_OBSTACLE &&
            !last_scan[j].meshed && !first_scan &&
            last_scan[j].range < max_range)
            write_point_glv(out_fp, last_scan[j].x, last_scan[j].y,
            last_scan[j].z);
        }
      }
      else {
      	write_color_glv(out_fp, 255, 255, 255);
        for (j = 0; j < num_readings; j++) {
          if ((pointclass[j] == TYPE_SAFE) || (pointclass[j] == TYPE_UNKNOWN))
          write_point_glv(out_fp, current_scan[j].x, current_scan[j].y,
			    current_scan[j].z);
        }

        write_color_glv(out_fp, 100, 100, 255);
        for (j = 0; j < num_readings; j++) {
          if (pointclass[j] == TYPE_OBSTACLE)
          write_point_glv(out_fp, current_scan[j].x, current_scan[j].y,
			    current_scan[j].z);
        }
      }

      first_scan = 0;
    }
  }

  fprintf(stderr, "\rProjecting points... (100%%)    \n");

  carmen_fclose(out_fp);
}
Esempio n. 21
0
int
main(int argc, char **argv)
{
  int width = 0, height = 0;
  double resolution = 0.0;
  char *filename = NULL;
  carmen_map_t map;
  int index;
  carmen_FILE *out_file;

  carmen_read_commandline_parameters(argc, argv);

  if (argc > 1)
    {
      if (strcmp(argv[argc-1], "-w") != 0 && 
	  strcmp(argv[argc-1], "-h") != 0 &&
	  strcmp(argv[argc-2], "-w") != 0 && 
	  strcmp(argv[argc-2], "-h") != 0)
	filename = argv[argc-1];	  
    }

  carmen_process_param_int("w", usage, &width);
  carmen_process_param_int("h", usage, &height);

  carmen_process_param_double("r", usage, &resolution);

  if (width <= 0)
    {
      printf("Map width: ");
      scanf("%d", &width);
    }

  if (height <= 0)
    {
      printf("Map height: ");
      scanf("%d", &height);
    }

  if (resolution <= 0)
    {
      printf("Map resolution: ");
      scanf("%lf", &resolution);
    }

  if (filename == NULL)
    {
      printf("Filename: ");
      filename = (char *)calloc(1024, sizeof(char));
      carmen_test_alloc(filename);
      scanf("%s", filename);
    }

  printf("Creating map %d x %d, resolution %.2f, storing in %s\n", 
	 width, height, resolution, filename);

  map.config.x_size = width;
  map.config.y_size = height;
  map.config.resolution = resolution;
  map.config.map_name = filename;

  map.complete_map = (float *)calloc(width*height, sizeof(float));
  carmen_test_alloc(map.complete_map);
  for (index = 0; index < width*height; index++)
    map.complete_map[index] = -1;

  map.map = (float **)calloc(width, sizeof(float *));
  carmen_test_alloc(map.map);

  for (index = 0; index < width; index++)    
    map.map[index] = map.complete_map+index*height;

  out_file = carmen_fopen(filename, "w");
  if (out_file == NULL)
      carmen_die("Couldn't open %s for writing map: %s\n", filename, 
		 strerror(errno));
  carmen_map_write_id(out_file);
  carmen_map_write_creator_chunk(out_file, "generate_blank", "blank map");
  carmen_map_write_gridmap_chunk(out_file, map.map, width, height, resolution);
  carmen_fclose(out_file);
      
  return 0;
}