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);
}
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);
}
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);
}
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));
}
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));
}
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));
}
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);
}
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);
}
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);
}
