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(¤t_pos);
gps_msg->validity = CLF_READ_INT(¤t_pos);
gps_msg->utc = CLF_READ_DOUBLE(¤t_pos);
gps_msg->latitude_dm = CLF_READ_DOUBLE(¤t_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(¤t_pos);
gps_msg->longitude_dm = CLF_READ_DOUBLE(¤t_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(¤t_pos);
gps_msg->speed = CLF_READ_DOUBLE(¤t_pos);
gps_msg->true_course = CLF_READ_DOUBLE(¤t_pos);
gps_msg->variation = CLF_READ_DOUBLE(¤t_pos);
current_pos = carmen_next_word(current_pos);
gps_msg->var_dir = CLF_READ_CHAR(¤t_pos);
gps_msg->date = CLF_READ_INT(¤t_pos);
gps_msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&gps_msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
gps_msg->utc = CLF_READ_DOUBLE(¤t_pos);
gps_msg->latitude_dm = CLF_READ_DOUBLE(¤t_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(¤t_pos);
gps_msg->longitude_dm = CLF_READ_DOUBLE(¤t_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(¤t_pos);
gps_msg->gps_quality = CLF_READ_INT(¤t_pos);
gps_msg->num_satellites = CLF_READ_INT(¤t_pos);
gps_msg->hdop = CLF_READ_DOUBLE(¤t_pos);
gps_msg->sea_level = CLF_READ_DOUBLE(¤t_pos);
gps_msg->altitude = CLF_READ_DOUBLE(¤t_pos);
gps_msg->geo_sea_level = CLF_READ_DOUBLE(¤t_pos);
gps_msg->geo_sep = CLF_READ_DOUBLE(¤t_pos);
gps_msg->data_age = CLF_READ_INT(¤t_pos);
gps_msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&gps_msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
msg->accY = CLF_READ_DOUBLE(¤t_pos);
msg->accZ = CLF_READ_DOUBLE(¤t_pos);
msg->q0 = CLF_READ_DOUBLE(¤t_pos);
msg->q1 = CLF_READ_DOUBLE(¤t_pos);
msg->q2 = CLF_READ_DOUBLE(¤t_pos);
msg->q3 = CLF_READ_DOUBLE(¤t_pos);
msg->magX = CLF_READ_DOUBLE(¤t_pos);
msg->magY = CLF_READ_DOUBLE(¤t_pos);
msg->magZ = CLF_READ_DOUBLE(¤t_pos);
msg->gyroX = CLF_READ_DOUBLE(¤t_pos);
msg->gyroY = CLF_READ_DOUBLE(¤t_pos);
msg->gyroZ = CLF_READ_DOUBLE(¤t_pos);
msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
arm->num_joints = CLF_READ_INT(¤t_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(¤t_pos);
arm->num_currents = CLF_READ_INT(¤t_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(¤t_pos);
arm->num_vels = CLF_READ_INT(¤t_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(¤t_pos);
arm->gripper_closed = CLF_READ_INT(¤t_pos);
arm->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&arm->host, ¤t_pos);
return current_pos;
}
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;
}
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;
}
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(¤t_pos);
msg->rv = CLF_READ_DOUBLE(¤t_pos);
msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
msg->theta = CLF_READ_DOUBLE(¤t_pos);
msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&msg->host, ¤t_pos);
return current_pos;
}
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(¤t_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(¤t_pos);
laser->tooclose[i] = 0;
}
laser->laser_pose.x = CLF_READ_DOUBLE(¤t_pos);
laser->laser_pose.y = CLF_READ_DOUBLE(¤t_pos);
laser->laser_pose.theta = CLF_READ_DOUBLE(¤t_pos);
laser->robot_pose.x = CLF_READ_DOUBLE(¤t_pos);
laser->robot_pose.y = CLF_READ_DOUBLE(¤t_pos);
laser->robot_pose.theta = CLF_READ_DOUBLE(¤t_pos);
laser->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&laser->host, ¤t_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;
}
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(¤t_pos);
ptstat->tilt = CLF_READ_DOUBLE(¤t_pos);
ptstat->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&ptstat->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
scanmark->laserid = CLF_READ_INT(¤t_pos);
scanmark->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&scanmark->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
truepos->truepose.y = CLF_READ_DOUBLE(¤t_pos);
truepos->truepose.theta = CLF_READ_DOUBLE(¤t_pos);
truepos->odometrypose.x = CLF_READ_DOUBLE(¤t_pos);
truepos->odometrypose.y = CLF_READ_DOUBLE(¤t_pos);
truepos->odometrypose.theta = CLF_READ_DOUBLE(¤t_pos);
truepos->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&truepos->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
odometry->y = CLF_READ_DOUBLE(¤t_pos);
odometry->theta = CLF_READ_DOUBLE(¤t_pos);
odometry->tv = CLF_READ_DOUBLE(¤t_pos);
odometry->rv = CLF_READ_DOUBLE(¤t_pos);
odometry->acceleration = CLF_READ_DOUBLE(¤t_pos);
odometry->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&odometry->host, ¤t_pos);
return current_pos;
}
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(¤t_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(¤t_pos);
for (i = 0; i < bumper_msg->num_bumpers; ++i) {
bumper_msg->bumper_offsets[i].x = CLF_READ_DOUBLE(¤t_pos);
bumper_msg->bumper_offsets[i].y = CLF_READ_DOUBLE(¤t_pos);
}
bumper_msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&bumper_msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
msg->robot_position.y = CLF_READ_DOUBLE(¤t_pos);
msg->robot_position.theta = CLF_READ_DOUBLE(¤t_pos);
msg->robot_position.t_vel = CLF_READ_DOUBLE(¤t_pos);
msg->robot_position.r_vel = CLF_READ_DOUBLE(¤t_pos);
int length = CLF_READ_INT(¤t_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(¤t_pos);
msg->trajectory[i].y = CLF_READ_DOUBLE(¤t_pos);
msg->trajectory[i].theta = CLF_READ_DOUBLE(¤t_pos);
msg->trajectory[i].t_vel = CLF_READ_DOUBLE(¤t_pos);
msg->trajectory[i].r_vel = CLF_READ_DOUBLE(¤t_pos);
}
msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
laserpos->x = CLF_READ_DOUBLE(¤t_pos);
laserpos->y = CLF_READ_DOUBLE(¤t_pos);
laserpos->z = CLF_READ_DOUBLE(¤t_pos);
laserpos->phi = CLF_READ_DOUBLE(¤t_pos);
laserpos->theta = CLF_READ_DOUBLE(¤t_pos);
laserpos->psi = CLF_READ_DOUBLE(¤t_pos);
laserpos->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&laserpos->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
sonar_msg->num_sonars = CLF_READ_INT(¤t_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(¤t_pos);
for (i = 0; i < sonar_msg->num_sonars; ++i) {
sonar_msg->sonar_offsets[i].x = CLF_READ_DOUBLE(¤t_pos);
sonar_msg->sonar_offsets[i].y = CLF_READ_DOUBLE(¤t_pos);
sonar_msg->sonar_offsets[i].theta = CLF_READ_DOUBLE(¤t_pos);
}
sonar_msg->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&sonar_msg->host, ¤t_pos);
return current_pos;
}
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(¤t_pos);
globalpos->globalpos.y = CLF_READ_DOUBLE(¤t_pos);
globalpos->globalpos.theta = CLF_READ_DOUBLE(¤t_pos);
globalpos->globalpos_std.x = 0;
globalpos->globalpos_std.y = 0;
globalpos->globalpos_std.theta = 0;
globalpos->odometrypos.x = CLF_READ_DOUBLE(¤t_pos);
globalpos->odometrypos.y = CLF_READ_DOUBLE(¤t_pos);
globalpos->odometrypos.theta = CLF_READ_DOUBLE(¤t_pos);
globalpos->globalpos_xy_cov = 0;
globalpos->converged = 1;
globalpos->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&globalpos->host, ¤t_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,
¤t_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);
}
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);
}