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;
}
示例#2
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;
}
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;
}