Exemple #1
0
char *carmen_string_to_gps_gprmc_message(char *string,
					 carmen_gps_gprmc_message *gps_msg)
{
  char *current_pos = string;
  
  if (strncmp(current_pos, "NMEARMC ", 8) == 0)
    current_pos = carmen_next_word(current_pos); 
  
  gps_msg->nr               = CLF_READ_INT(&current_pos);
  gps_msg->validity         = CLF_READ_INT(&current_pos);
  gps_msg->utc              = CLF_READ_DOUBLE(&current_pos);
  gps_msg->latitude_dm      = CLF_READ_DOUBLE(&current_pos);
  gps_msg->latitude         = carmen_global_convert_degmin_to_double(gps_msg->latitude_dm);
  current_pos = carmen_next_word(current_pos); 
  gps_msg->lat_orient       = CLF_READ_CHAR(&current_pos);
  gps_msg->longitude_dm     = CLF_READ_DOUBLE(&current_pos);
  gps_msg->longitude        = carmen_global_convert_degmin_to_double(gps_msg->longitude_dm);
  current_pos = carmen_next_word(current_pos); 
  gps_msg->long_orient      = CLF_READ_CHAR(&current_pos);
  gps_msg->speed            = CLF_READ_DOUBLE(&current_pos);
  gps_msg->true_course      = CLF_READ_DOUBLE(&current_pos);
  gps_msg->variation        = CLF_READ_DOUBLE(&current_pos);
  current_pos = carmen_next_word(current_pos); 
  gps_msg->var_dir          = CLF_READ_CHAR(&current_pos);
  gps_msg->date             = CLF_READ_INT(&current_pos);
  gps_msg->timestamp        = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&gps_msg->host, &current_pos);

  return current_pos;
}
Exemple #2
0
char *carmen_string_to_gps_gpgga_message(char *string,
				       carmen_gps_gpgga_message *gps_msg)
{
  char *current_pos = string;
  
  if (strncmp(current_pos, "NMEAGGA ", 8) == 0)
    current_pos = carmen_next_word(current_pos); 
  
  gps_msg->nr               = CLF_READ_INT(&current_pos);
  gps_msg->utc              = CLF_READ_DOUBLE(&current_pos);
  gps_msg->latitude_dm      = CLF_READ_DOUBLE(&current_pos);
  gps_msg->latitude         = carmen_global_convert_degmin_to_double(gps_msg->latitude_dm);
  current_pos = carmen_next_word(current_pos); 
  gps_msg->lat_orient       = CLF_READ_CHAR(&current_pos);
  gps_msg->longitude_dm     = CLF_READ_DOUBLE(&current_pos);
  gps_msg->longitude        = carmen_global_convert_degmin_to_double(gps_msg->longitude_dm);
  current_pos = carmen_next_word(current_pos); 
  gps_msg->long_orient      = CLF_READ_CHAR(&current_pos);
  gps_msg->gps_quality      = CLF_READ_INT(&current_pos);
  gps_msg->num_satellites   = CLF_READ_INT(&current_pos);
  gps_msg->hdop             = CLF_READ_DOUBLE(&current_pos);
  gps_msg->sea_level        = CLF_READ_DOUBLE(&current_pos);
  gps_msg->altitude         = CLF_READ_DOUBLE(&current_pos);
  gps_msg->geo_sea_level    = CLF_READ_DOUBLE(&current_pos);
  gps_msg->geo_sep          = CLF_READ_DOUBLE(&current_pos);
  gps_msg->data_age         = CLF_READ_INT(&current_pos);
  gps_msg->timestamp        = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&gps_msg->host, &current_pos);

  return current_pos;
}
Exemple #3
0
char* carmen_string_to_imu_message(char* string, carmen_imu_message* msg)
{
  char* current_pos = string;
  if (strncmp(current_pos, "IMU ", 4) == 0)
    current_pos = carmen_next_word(current_pos);

  msg->accX = CLF_READ_DOUBLE(&current_pos);
  msg->accY = CLF_READ_DOUBLE(&current_pos);
  msg->accZ = CLF_READ_DOUBLE(&current_pos);

  msg->q0 = CLF_READ_DOUBLE(&current_pos);
  msg->q1 = CLF_READ_DOUBLE(&current_pos);
  msg->q2 = CLF_READ_DOUBLE(&current_pos);
  msg->q3 = CLF_READ_DOUBLE(&current_pos);

  msg->magX = CLF_READ_DOUBLE(&current_pos);
  msg->magY = CLF_READ_DOUBLE(&current_pos);
  msg->magZ = CLF_READ_DOUBLE(&current_pos);

  msg->gyroX = CLF_READ_DOUBLE(&current_pos);
  msg->gyroY = CLF_READ_DOUBLE(&current_pos);
  msg->gyroZ = CLF_READ_DOUBLE(&current_pos);

  msg->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&msg->host, &current_pos);

  return current_pos;
}
Exemple #4
0
char *carmen_string_to_arm_state_message(char *string,
					 carmen_arm_state_message *arm)
{
  int i;
  char *current_pos = string;

  if (strncmp(current_pos, "ARM ", 4) == 0)
    current_pos = carmen_next_word(current_pos); 
  
  arm->flags = CLF_READ_INT(&current_pos);
  arm->num_joints = CLF_READ_INT(&current_pos);
  arm->joint_angles = (double *) realloc(arm->joint_angles, arm->num_joints*sizeof(double));
  carmen_test_alloc(arm->joint_angles);
  for (i = 0; i < arm->num_joints; i++)
    arm->joint_angles[i] = CLF_READ_DOUBLE(&current_pos);
  arm->num_currents = CLF_READ_INT(&current_pos);
  arm->joint_currents = (double *) realloc(arm->joint_currents, arm->num_currents*sizeof(double));
  carmen_test_alloc(arm->joint_currents);
  for (i = 0; i < arm->num_currents; i++)
    arm->joint_currents[i] = CLF_READ_DOUBLE(&current_pos);
  arm->num_vels = CLF_READ_INT(&current_pos);
  arm->joint_angular_vels = (double *) realloc(arm->joint_angular_vels, 
					       arm->num_vels*sizeof(double));
  carmen_test_alloc(arm->joint_angular_vels);
  for (i = 0; i < arm->num_vels; i++)
    arm->joint_angular_vels[i] = CLF_READ_DOUBLE(&current_pos);
  arm->gripper_closed = CLF_READ_INT(&current_pos);

  arm->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&arm->host, &current_pos);
  return current_pos;
}
Exemple #5
0
carmen_inline char *carmen_next_n_words(char *str, int n)
{
  int i;
  char *result;

  result = str;
  for(i = 0; i < n; i++)
    result = carmen_next_word(result);
  return result;
}
Exemple #6
0
int read_message(int message_num, int publish, int no_wait)
{
	//  char *line[MAX_LINE_LENGTH];
	char *line;
	char *current_pos;
	int i, j;
	char command[100];
	static double last_update = 0;
	double current_time;

	line = (char *) malloc(MAX_LINE_LENGTH * sizeof(char));
	if (line == NULL)
		carmen_die("Could not alloc memory in playback.c:read_message()\n");

	carmen_logfile_read_line(logfile_index, logfile, message_num,
			MAX_LINE_LENGTH, line);
	current_pos = carmen_next_word(line);

	for(i = 0; i < (int)(sizeof(logger_callbacks) /
			sizeof(logger_callback_t)); i++) {
		/* copy the command over */
		j = 0;
		while(line[j] != ' ') {
			command[j] = line[j];
			j++;
		}
		command[j] = '\0';
		if(strncmp(command, logger_callbacks[i].logger_message_name, j) == 0) {
			if(!basic_messages || !logger_callbacks[i].interpreted) {
				current_pos =
					logger_callbacks[i].conv_func(current_pos,
							logger_callbacks[i].message_data);
				playback_timestamp = atof(current_pos);
				//printf("command = %s, playback_timestamp = %lf\n", command, playback_timestamp);
				if(publish) {
					current_time = carmen_get_time();
					if(current_time - last_update > 0.2) {
						print_playback_status();
						last_update = current_time;
					}
					if (!no_wait)
						wait_for_timestamp(playback_timestamp);
					IPC_publishData(logger_callbacks[i].ipc_message_name,
							logger_callbacks[i].message_data);
				}
				/* return 1 if it is a front laser message */
				free(line);
				return (strcmp(command, "FLASER") == 0);
			}
		}
	}

	free(line);
	return 0;
}
Exemple #7
0
char *carmen_string_to_base_velocity_message(char *string, carmen_base_velocity_message *msg)
{
	char *current_pos = string;

	if (strncmp(current_pos, "BASEVELOCITY ", 13) == 0)
   	current_pos = carmen_next_word(current_pos); 
  
  	msg->tv = CLF_READ_DOUBLE(&current_pos);
  	msg->rv = CLF_READ_DOUBLE(&current_pos);
  	msg->timestamp = CLF_READ_DOUBLE(&current_pos);
   copy_host_string(&msg->host, &current_pos);
	return current_pos;
}
Exemple #8
0
char *carmen_string_to_robot_vector_move_message(char *string, carmen_robot_vector_move_message *msg)
{
	char *current_pos = string;

	if (strncmp(current_pos, "VECTORMOVE ", 11) == 0)
   	current_pos = carmen_next_word(current_pos); 

  	msg->distance = CLF_READ_DOUBLE(&current_pos);
  	msg->theta = CLF_READ_DOUBLE(&current_pos);
  	msg->timestamp = CLF_READ_DOUBLE(&current_pos);
   copy_host_string(&msg->host, &current_pos);
	return current_pos;
}
Exemple #9
0
char *carmen_string_to_robot_laser_message_orig(char *string,
						carmen_robot_laser_message
						*laser)
{
  char *current_pos = string;
  int i, num_readings;

  if (strncmp(current_pos, "FLASER", 6) == 0 || 
      strncmp(current_pos, "RLASER", 6) == 0)
    current_pos = carmen_next_word(current_pos); 

  num_readings = CLF_READ_INT(&current_pos);
  if(laser->num_readings != num_readings) {
    laser->num_readings = num_readings;
    laser->range = (float *)realloc(laser->range, laser->num_readings * 
				    sizeof(float));
    carmen_test_alloc(laser->range);
    laser->tooclose = (char *)realloc(laser->tooclose, laser->num_readings);
    carmen_test_alloc(laser->tooclose);
  }
  for(i = 0; i < laser->num_readings; i++) {
    laser->range[i] = CLF_READ_DOUBLE(&current_pos);
    laser->tooclose[i] = 0;
  }

  laser->laser_pose.x = CLF_READ_DOUBLE(&current_pos);
  laser->laser_pose.y = CLF_READ_DOUBLE(&current_pos);
  laser->laser_pose.theta = CLF_READ_DOUBLE(&current_pos);
  laser->robot_pose.x = CLF_READ_DOUBLE(&current_pos);
  laser->robot_pose.y = CLF_READ_DOUBLE(&current_pos);
  laser->robot_pose.theta = CLF_READ_DOUBLE(&current_pos);
  laser->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&laser->host, &current_pos);

  /* fill in remission with nothing */
  laser->num_remissions = 0;
  laser->remission = NULL;

  /* guess at fields */
  laser->config.laser_type = SICK_LMS;
  laser->config.fov = carmen_laser_guess_fov(laser->num_readings);
  laser->config.start_angle = -M_PI / 2.0;
  laser->config.angular_resolution = 
    carmen_laser_guess_angle_increment(laser->num_readings);
  laser->config.maximum_range = 80.0;
  laser->config.accuracy = 0.01;
  laser->config.remission_mode = 0;

  return current_pos;
}
Exemple #10
0
char* carmen_string_to_pantilt_status_message(char* string, carmen_pantilt_status_message* ptstat) {

  char *current_pos = string;
  
  if (strncmp(current_pos, "PANTILT ", 8) == 0) {
    current_pos = carmen_next_word(current_pos);
  }
  
  ptstat->pan = CLF_READ_DOUBLE(&current_pos);
  ptstat->tilt = CLF_READ_DOUBLE(&current_pos);

  ptstat->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&ptstat->host, &current_pos);

  return current_pos;
}
Exemple #11
0
char* carmen_string_to_pantilt_scanmark_message(char* string, carmen_pantilt_scanmark_message* scanmark) {

  char *current_pos = string;

  if (strncmp(current_pos, "SCANMARK ", 9) == 0) {
    current_pos = carmen_next_word(current_pos);
  }

  scanmark->type = CLF_READ_INT(&current_pos);
  scanmark->laserid = CLF_READ_INT(&current_pos);

  scanmark->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&scanmark->host, &current_pos);

  return current_pos;
}
Exemple #12
0
char *carmen_string_to_simulator_truepos_message(char *string,
						 carmen_simulator_truepos_message *truepos)
{
  char *current_pos = string;

  if (strncmp(current_pos, "TRUEPOS ", 8) == 0)
    current_pos = carmen_next_word(current_pos); 

 
  truepos->truepose.x = CLF_READ_DOUBLE(&current_pos);
  truepos->truepose.y = CLF_READ_DOUBLE(&current_pos);
  truepos->truepose.theta = CLF_READ_DOUBLE(&current_pos);
  truepos->odometrypose.x = CLF_READ_DOUBLE(&current_pos);
  truepos->odometrypose.y = CLF_READ_DOUBLE(&current_pos);
  truepos->odometrypose.theta = CLF_READ_DOUBLE(&current_pos);
  truepos->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&truepos->host, &current_pos);
  return current_pos;
}
Exemple #13
0
char *carmen_string_to_base_odometry_message(char *string,
					     carmen_base_odometry_message
					     *odometry)
{
  char *current_pos = string;

  if (strncmp(current_pos, "ODOM ", 5) == 0)
    current_pos = carmen_next_word(current_pos); 
  
  odometry->x = CLF_READ_DOUBLE(&current_pos);
  odometry->y = CLF_READ_DOUBLE(&current_pos);
  odometry->theta = CLF_READ_DOUBLE(&current_pos);
  odometry->tv = CLF_READ_DOUBLE(&current_pos);
  odometry->rv = CLF_READ_DOUBLE(&current_pos);
  odometry->acceleration = CLF_READ_DOUBLE(&current_pos);
  odometry->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&odometry->host, &current_pos);
  return current_pos;
}
Exemple #14
0
char* carmen_string_to_base_bumper_message(char* string, carmen_base_bumper_message* bumper_msg)
{
  int i;
  char* current_pos = string;
  if (strncmp(current_pos, "BUMPER ", 7) == 0)
    current_pos = carmen_next_word(current_pos);

  bumper_msg->num_bumpers = CLF_READ_INT(&current_pos);
  bumper_msg->state = (unsigned char*) realloc(bumper_msg->state, sizeof(unsigned char) * bumper_msg->num_bumpers);
  bumper_msg->bumper_offsets = (carmen_position_t*) realloc(bumper_msg->bumper_offsets, sizeof(carmen_position_t) * bumper_msg->num_bumpers);
  for (i = 0; i < bumper_msg->num_bumpers; ++i)
    bumper_msg->state[i] = CLF_READ_INT(&current_pos);
  for (i = 0; i < bumper_msg->num_bumpers; ++i) {
    bumper_msg->bumper_offsets[i].x = CLF_READ_DOUBLE(&current_pos);
    bumper_msg->bumper_offsets[i].y = CLF_READ_DOUBLE(&current_pos);
  }
  bumper_msg->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&bumper_msg->host, &current_pos);

  return current_pos;
}
Exemple #15
0
char *carmen_string_to_robot_follow_trajectory_message(char *string, carmen_robot_follow_trajectory_message *msg)
{
	char *current_pos = string;
	
	if (strncmp(current_pos, "ROBOTVELOCITY ", 14) == 0)
   	current_pos = carmen_next_word(current_pos); 

	msg->robot_position.x = CLF_READ_DOUBLE(&current_pos);
	msg->robot_position.y = CLF_READ_DOUBLE(&current_pos);
	msg->robot_position.theta = CLF_READ_DOUBLE(&current_pos);
	msg->robot_position.t_vel = CLF_READ_DOUBLE(&current_pos);
	msg->robot_position.r_vel = CLF_READ_DOUBLE(&current_pos);

	int length = CLF_READ_INT(&current_pos);
	
   if(msg->trajectory_length != length) {
    msg->trajectory_length = length;
    msg->trajectory = (carmen_traj_point_t *)realloc(msg->trajectory, length * 
				   sizeof(carmen_traj_point_t));
    carmen_test_alloc(msg->trajectory);
  }



	int i;
	for (i=0; i<msg->trajectory_length; i++) 
	{
		msg->trajectory[i].x = CLF_READ_DOUBLE(&current_pos);
		msg->trajectory[i].y = CLF_READ_DOUBLE(&current_pos);
		msg->trajectory[i].theta = CLF_READ_DOUBLE(&current_pos);
		msg->trajectory[i].t_vel = CLF_READ_DOUBLE(&current_pos);
		msg->trajectory[i].r_vel = CLF_READ_DOUBLE(&current_pos);
	}

  	msg->timestamp = CLF_READ_DOUBLE(&current_pos);
   copy_host_string(&msg->host, &current_pos);
	return current_pos;
}
Exemple #16
0
char* carmen_string_to_pantilt_laserpos_message(char* string, carmen_pantilt_laserpos_message* laserpos) {

  char *current_pos = string;

  if (strncmp(current_pos, "POSITIONLASER ", 14) == 0) {
    current_pos = carmen_next_word(current_pos);
  } 
  
  laserpos->id = CLF_READ_INT(&current_pos);
  
  laserpos->x = CLF_READ_DOUBLE(&current_pos);
  laserpos->y = CLF_READ_DOUBLE(&current_pos);
  laserpos->z = CLF_READ_DOUBLE(&current_pos);

  laserpos->phi = CLF_READ_DOUBLE(&current_pos);
  laserpos->theta = CLF_READ_DOUBLE(&current_pos);
  laserpos->psi = CLF_READ_DOUBLE(&current_pos);
  
  laserpos->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&laserpos->host, &current_pos);
  
  return current_pos;
}
Exemple #17
0
char* carmen_string_to_base_sonar_message(char* string, carmen_base_sonar_message* sonar_msg)
{
  int i;
  char* current_pos = string;
  if (strncmp(current_pos, "SONAR ", 6) == 0)
    current_pos = carmen_next_word(current_pos);

  sonar_msg->cone_angle = CLF_READ_DOUBLE(&current_pos);
  sonar_msg->num_sonars = CLF_READ_INT(&current_pos);
  sonar_msg->sonar_offsets = (carmen_point_p) realloc(sonar_msg->sonar_offsets, sizeof(carmen_point_t) * sonar_msg->num_sonars);
  sonar_msg->range = (double*) realloc(sonar_msg->range, sizeof(double) * sonar_msg->num_sonars);
  for (i = 0; i < sonar_msg->num_sonars; ++i)
    sonar_msg->range[i] = CLF_READ_DOUBLE(&current_pos);
  for (i = 0; i < sonar_msg->num_sonars; ++i) {
    sonar_msg->sonar_offsets[i].x     = CLF_READ_DOUBLE(&current_pos);
    sonar_msg->sonar_offsets[i].y     = CLF_READ_DOUBLE(&current_pos);
    sonar_msg->sonar_offsets[i].theta = CLF_READ_DOUBLE(&current_pos);
  }
  sonar_msg->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&sonar_msg->host, &current_pos);

  return current_pos;
}
Exemple #18
0
char *carmen_string_to_localize_globalpos_message(char *string, carmen_localize_globalpos_message *globalpos)
{
  char *current_pos = string;

  if (strncmp(current_pos, "GLOBALPOS ", 10) == 0)
    current_pos = carmen_next_word(current_pos);

  globalpos->globalpos.x = CLF_READ_DOUBLE(&current_pos);
  globalpos->globalpos.y = CLF_READ_DOUBLE(&current_pos);
  globalpos->globalpos.theta = CLF_READ_DOUBLE(&current_pos);
  globalpos->globalpos_std.x = 0;
  globalpos->globalpos_std.y = 0;
  globalpos->globalpos_std.theta = 0;
  globalpos->odometrypos.x = CLF_READ_DOUBLE(&current_pos);
  globalpos->odometrypos.y = CLF_READ_DOUBLE(&current_pos);
  globalpos->odometrypos.theta = CLF_READ_DOUBLE(&current_pos);

  globalpos->globalpos_xy_cov = 0;
  globalpos->converged = 1;

  globalpos->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&globalpos->host, &current_pos);
  return current_pos;
}
void read_firecam_logfile(char *filename, logdata_p logdata) {
  int buffer_pos, buffer_length, offset = 0;
  int linecount = 0, mark, n;
  long int nread, log_bytes = 0;
  carmen_FILE *log_fp = NULL;
  char *current_pos;
  char buffer[10000];

  /* initialize logdata structure */
  logdata->num_frame = 0;
  logdata->max_frame = 1000;
  logdata->frame = (firecam_frame_p)calloc(logdata->max_frame,
    sizeof(firecam_frame_t));
  carmen_test_alloc(logdata->frame);

  /* compute total number of bytes in logfile */
  log_fp = carmen_fopen(filename, "r");
  if (log_fp == NULL)
    carmen_die("Error: could not open file %s for reading.\n", filename);

  do {
    nread = carmen_fread(buffer, 1, 10000, log_fp);
    log_bytes += nread;
  }
  while (nread > 0);
  carmen_fseek(log_fp, 0L, SEEK_SET);

  /* read the logfile */
  buffer_pos = 0;
  buffer_length = carmen_fread(buffer, 1, 10000, log_fp);

  while (buffer_length > 0) {
    mark = buffer_pos;
    while ((mark < buffer_length) && (buffer[mark] != '\n'))
      ++mark;

    if (mark == buffer_length) {
      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, log_fp);
      buffer_length += n;
    }
    else {
      ++linecount;
      if (linecount % 100 == 0)
        fprintf(stderr, "\rReading log file %s... (%.0f%%)  ", filename,
        (offset+buffer_pos)/(float)log_bytes*100.0);
      buffer[mark] = '\0';

      if (!strncmp(buffer+buffer_pos, "FIRECAMFRAME", 12)) {
        if (logdata->num_frame == logdata->max_frame) {
          logdata->max_frame += 1000;
          logdata->frame = (firecam_frame_p)realloc(logdata->frame,
            logdata->max_frame*sizeof(firecam_frame_t));
          carmen_test_alloc(logdata->frame);
        }

        current_pos = buffer+buffer_pos;
        current_pos = carmen_next_word(current_pos);
        logdata->frame[logdata->num_frame].cam_id = atoi(current_pos);
        copy_filename_string(&logdata->frame[logdata->num_frame].filename,
          &current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->frame[logdata->num_frame].timestamp = atof(current_pos);

        ++logdata->num_frame;
      }

      buffer_pos = mark+1;
    }
  }

  fprintf(stderr, "\nRead %d FRAME\n", logdata->num_frame);
}
Exemple #20
0
void read_nsick_logfile(char *filename, logdata_p logdata) {
  int buffer_pos, buffer_length, offset = 0;
  int linecount = 0, mark, n, i;
  long int nread, log_bytes = 0;
  carmen_FILE *log_fp = NULL;
  char *current_pos;
  char buffer[10000];
  int laser_num_readings;
  double laser_start_angle, laser_fov;

  /* initialize logdata structure */
  logdata->num_laser = 0;
  logdata->max_laser = 1000;
  logdata->laser = (laser_scan_p)calloc(logdata->max_laser,
    sizeof(laser_scan_t));
  carmen_test_alloc(logdata->laser);
  logdata->num_pos = 0;
  logdata->max_pos = 1000;
  logdata->pos = (laser_pos_p)calloc(logdata->max_pos, sizeof(laser_pos_t));
  carmen_test_alloc(logdata->pos);

  /* compute total number of bytes in logfile */
  log_fp = carmen_fopen(filename, "r");
  if (log_fp == NULL)
    carmen_die("Error: could not open file %s for reading.\n", filename);

  do {
    nread = carmen_fread(buffer, 1, 10000, log_fp);
    log_bytes += nread;
  }
  while (nread > 0);
  carmen_fseek(log_fp, 0L, SEEK_SET);

  /* read the logfile */
  buffer_pos = 0;
  buffer_length = carmen_fread(buffer, 1, 10000, log_fp);

  while (buffer_length > 0) {
    mark = buffer_pos;
    while ((mark < buffer_length) && (buffer[mark] != '\n'))
      ++mark;

    if (mark == buffer_length) {
      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, log_fp);
      buffer_length += n;
    }
    else {
      ++linecount;
      if (linecount % 100 == 0)
        fprintf(stderr, "\rReading log file %s... (%.0f%%)  ", filename,
        (offset+buffer_pos)/(float)log_bytes*100.0);
      buffer[mark] = '\0';

      if (!strncmp(buffer+buffer_pos, "NSICKLASERPOS", 13)) {
        if (logdata->num_pos == logdata->max_pos) {
          logdata->max_pos += 1000;
          logdata->pos = (laser_pos_p)realloc(logdata->pos,
            logdata->max_pos*sizeof(laser_pos_t));
          carmen_test_alloc(logdata->pos);
        }

        current_pos = buffer+buffer_pos;
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].laser_num = atoi(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].x = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].y = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].z = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].yaw = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].pitch = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].roll = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        logdata->pos[logdata->num_pos].timestamp = atof(current_pos);

        ++logdata->num_pos;
      }
      else if (logdata->num_pos &&
        !strncmp(buffer+buffer_pos, "NSICKLASER", 10) &&
        buffer[buffer_pos+10] ==
          '0'+logdata->pos[logdata->num_pos-1].laser_num) {
        if (logdata->num_laser == logdata->max_laser) {
          logdata->max_laser += 1000;
          logdata->laser = (laser_scan_p)realloc(logdata->laser,
            logdata->max_laser*sizeof(laser_scan_t));
          carmen_test_alloc(logdata->laser);
        }

        current_pos = buffer+buffer_pos;
        logdata->laser[logdata->num_laser].laser_num =
          logdata->pos[logdata->num_pos].laser_num;

        current_pos = carmen_next_n_words(current_pos, 2);
        laser_start_angle = atof(current_pos);
        current_pos = carmen_next_word(current_pos);
        laser_fov = atof(current_pos);
        current_pos = carmen_next_n_words(current_pos, 5);
        
        laser_num_readings = atoi(current_pos);
        logdata->laser[logdata->num_laser].num_readings = laser_num_readings;
        logdata->laser[logdata->num_laser].start_angle = laser_start_angle;
        logdata->laser[logdata->num_laser].fov = laser_fov;
        logdata->laser[logdata->num_laser].range =
          (float*)calloc(laser_num_readings, sizeof(float));
        carmen_test_alloc(logdata->laser[logdata->num_laser].range);
        logdata->laser[logdata->num_laser].endpoint_x =
          (float*)calloc(laser_num_readings, sizeof(float));
        carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_x);
        logdata->laser[logdata->num_laser].endpoint_y =
          (float*)calloc(laser_num_readings, sizeof(float));
        carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_y);
        logdata->laser[logdata->num_laser].endpoint_z =
          (float*)calloc(laser_num_readings, sizeof(float));
        carmen_test_alloc(logdata->laser[logdata->num_laser].endpoint_z);
        current_pos = carmen_next_word(current_pos);
        for (i = 0; i < laser_num_readings; i++) {
          logdata->laser[logdata->num_laser].range[i] = atof(current_pos);
          current_pos = carmen_next_word(current_pos);
        }

        current_pos = carmen_next_word(current_pos);
        laser_num_readings = atoi(current_pos);
        logdata->laser[logdata->num_laser].timestamp = atof(current_pos);

        logdata->num_laser++;
      }

      buffer_pos = mark+1;
    }
  }

  fprintf(stderr, "\nRead %d LASER - %d POS\n",
	  logdata->num_laser, logdata->num_pos);
}