示例#1
0
int
main(int argc, char *argv[])
{
  int                         gps_nr = 0;
  SerialDevice                dev;
  carmen_gps_gpgga_message    gpgga;
  carmen_gps_gprmc_message    gprmc;

  carmen_erase_structure(&gpgga, sizeof(carmen_gps_gpgga_message) );
  carmen_erase_structure(&gprmc, sizeof(carmen_gps_gprmc_message) );
  
  gpgga.host = carmen_get_host();
  gprmc.host = carmen_get_host();
  DEVICE_init_params( &dev );

  carmen_ipc_initialize( argc, argv );
  ipc_initialize_messages();
 
  read_parameters( &dev, argc, argv );

  carmen_extern_gpgga_ptr = &gpgga;
  carmen_extern_gpgga_ptr->nr = gps_nr;
  carmen_extern_gprmc_ptr = &gprmc;
  carmen_extern_gprmc_ptr->nr = gps_nr;

  fprintf( stderr, "INFO: ************************\n" );
  fprintf( stderr, "INFO: ********* GPS   ********\n" );
  fprintf( stderr, "INFO: ************************\n" );

  fprintf( stderr, "INFO: open device: %s\n", dev.ttyport );
  if (DEVICE_connect_port( &dev )<0) {
    fprintf( stderr, "ERROR: can't open device !!!\n\n" );
    exit(1);
  } else {
    fprintf( stderr, "INFO: done\n" );
  }

  while(TRUE) {

    if ( DEVICE_bytes_waiting( dev.fd )>10 ) {
      if (DEVICE_read_data( dev )) {
	gpgga.timestamp = carmen_get_time();
	gprmc.timestamp = carmen_get_time();

	ipc_publish_position();
      }
      usleep(100000);
    } else {
      carmen_ipc_sleep(0.25);
      //IPC_listen(0);
      //usleep(250000);
    }

  }

  return(0);
}
示例#2
0
char* TrueposMessage::fromString(char* s) {
  if (m_msg == NULL) {
    m_msg = new carmen_simulator_truepos_message;
    carmen_erase_structure(m_msg, sizeof(carmen_simulator_truepos_message));
  }
  return carmen_string_to_simulator_truepos_message(s, m_msg);
}
示例#3
0
char* LaserMessage::fromString(char* s) {
  if (m_msg == NULL) {
    m_msg = new carmen_laser_laser_message;
    carmen_erase_structure(m_msg, sizeof(carmen_laser_laser_message));
  }
  return carmen_string_to_laser_laser_message(s, m_msg);
}
示例#4
0
void TrueposMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_simulator_truepos_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_simulator_truepos_message));
}
示例#5
0
void OdometryMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_base_odometry_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_base_odometry_message));
}
示例#6
0
void IMUMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_imu_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_imu_message));
}
示例#7
0
char* RobotLaserMessage::fromString(char* s) {
  if (m_msg == NULL) {
    m_msg = new carmen_robot_laser_message;
    carmen_erase_structure(m_msg, sizeof(carmen_robot_laser_message));
  }

  if (!strncmp(s,"FLASER ", 7))
    return carmen_string_to_robot_laser_message_orig(s, m_msg);
  else if (!strncmp(s,"RLASER ", 7))
    return carmen_string_to_robot_laser_message_orig(s, m_msg);
  else 
    return carmen_string_to_robot_laser_message(s, m_msg);
}
示例#8
0
void LaserMessage::init(int num_readings, int num_remissions) {

  if (m_msg != NULL) {
    freeRange();
    freeRemission();
  }
  else {
    m_msg = new carmen_laser_laser_message;
    carmen_test_alloc(m_msg);
  }
  carmen_erase_structure(m_msg, sizeof(carmen_laser_laser_message));

  m_msg->config = LaserConfig();
  
  initRange(num_readings);
  initRemission(num_remissions);
}
示例#9
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);
  
}