void carmen_logwrite_write_kinect_video(carmen_kinect_video_message *kinect,
int kinect_num, carmen_FILE *outfile,
double timestamp)
{
int i, j;
carmen_fprintf(outfile, "RAW_KINECT_VIDEO%d ", kinect_num);
carmen_fprintf(outfile, "%d ", kinect->width);
carmen_fprintf(outfile, "%d ", kinect->height);
carmen_fprintf(outfile, "%d ", kinect->size);
if (hex_char_image_kinect == NULL)
{
hex_char_image_kinect = (char *) malloc((2 * kinect->size) * sizeof(char)); // Twice the number of bytes
for (i=0; i < 16; i++)
{
if (i <= 9)
int_to_nibble_hex[i] = '0' + i;
else
int_to_nibble_hex[i] = 'a' + i - 10;
}
}
for(i=j=0; i<(kinect->size); i++, j+=2)
{
hex_char_image_kinect[j] = GET_HIGH_ORDER_NIBBLE(kinect->video[i]);
hex_char_image_kinect[j+1] = GET_LOW_ORDER_NIBBLE(kinect->video[i]);
}
carmen_fwrite(hex_char_image_kinect, (2 * kinect->size), 1, outfile);
carmen_fprintf(outfile, "%f %s %f\n", kinect->timestamp, kinect->host, timestamp);
}
void carmen_logwrite_write_laser_ldmrs(carmen_laser_ldmrs_message *laser,
int laser_num, carmen_FILE *outfile,
double timestamp)
{
int i;
(void)laser_num;
carmen_fprintf(outfile, "LASER_LDMRS ");
carmen_fprintf(outfile, "%d %f %f %d %f %f %d ",
laser->scan_number,
laser->scan_start_time,
laser->scan_end_time,
laser->angle_ticks_per_rotation,
laser->start_angle,
laser->end_angle,
laser->scan_points);
for(i = 0; i < laser->scan_points; i++)
{
carmen_fprintf(outfile, "%f %f %f %d ",
laser->arraypoints[i].horizontal_angle,
laser->arraypoints[i].vertical_angle,
laser->arraypoints[i].radial_distance,
laser->arraypoints[i].flags);
}
carmen_fprintf(outfile, "%f %s %f\n", laser->timestamp,
laser->host, timestamp);
}
void carmen_logwrite_write_laser_ldmrs_objects(carmen_laser_ldmrs_objects_message *laser,
int laser_num, carmen_FILE *outfile,
double timestamp)
{
int i;
(void)laser_num;
carmen_fprintf(outfile, "LASER_LDMRS_OBJECTS ");
carmen_fprintf(outfile, "%d ",
laser->num_objects);
for(i = 0; i < laser->num_objects; i++)
{
carmen_fprintf(outfile,"%d %f %f %f %f %f %f %d ",
laser->objects_list[i].id,
laser->objects_list[i].x,
laser->objects_list[i].y,
laser->objects_list[i].lenght,
laser->objects_list[i].width,
laser->objects_list[i].velocity,
laser->objects_list[i].orientation,
laser->objects_list[i].classId);
}
carmen_fprintf(outfile, "%f %s %f\n", laser->timestamp,
laser->host, timestamp);
}
void nsick_writelog_write_nsick_laserpos(carmen_nsick_laserpos_message
*laserpos, carmen_FILE *outfile, double timestamp) {
carmen_fprintf(outfile, "NSICKLASERPOS %d ", laserpos->laser_id);
carmen_fprintf(outfile, "%f %f %f ", laserpos->x, laserpos->y, laserpos->z);
carmen_fprintf(outfile, "%f %f %f ", laserpos->yaw, laserpos->pitch,
laserpos->roll);
carmen_fprintf(outfile, "%lf %s %lf\n", laserpos->timestamp, laserpos->host,
timestamp);
}
void carmen_logwrite_write_pantilt_scanmark(carmen_pantilt_scanmark_message *scanmark,
carmen_FILE *outfile, double timestamp) {
carmen_fprintf(outfile, "SCANMARK ");
carmen_fprintf(outfile, "%d ", scanmark->type);
carmen_fprintf(outfile, "%d ", scanmark->laserid);
carmen_fprintf(outfile, "%lf %s %lf\n", scanmark->timestamp, scanmark->host, timestamp);
}
void carmen_logwrite_write_pantilt_laserpos(carmen_pantilt_laserpos_message *laserpos,
carmen_FILE *outfile, double timestamp){
carmen_fprintf(outfile, "POSITIONLASER %d ",laserpos->id);
carmen_fprintf(outfile, "%f %f %f ", laserpos->x, laserpos->y, laserpos->z);
carmen_fprintf(outfile, "%f %f %f ", laserpos->phi, laserpos->theta, laserpos->psi);
carmen_fprintf(outfile, "%lf %s %lf\n", laserpos->timestamp, laserpos->host, timestamp);
}
void carmen_logwrite_write_robot_ackerman_laser(carmen_robot_ackerman_laser_message *laser,
int laser_num, carmen_FILE *outfile,
double timestamp)
{
int i;
carmen_fprintf(outfile, "ROBOTLASER_ACK%d ", laser_num);
carmen_fprintf(outfile, "%d %f %f %f %f %f %d ", laser->config.laser_type,
laser->config.start_angle, laser->config.fov,
laser->config.angular_resolution,
laser->config.maximum_range,
laser->config.accuracy,
laser->config.remission_mode);
carmen_fprintf(outfile, "%d ", laser->num_readings);
for(i = 0; i < laser->num_readings; i++)
carmen_fprintf(outfile, "%.3f ", laser->range[i]);
carmen_fprintf(outfile, "%d ", laser->num_remissions);
for(i = 0; i < laser->num_remissions; i++)
carmen_fprintf(outfile, "%f ", laser->remission[i]);
carmen_fprintf(outfile, "%f %f %f %f %f %f ", laser->laser_pose.x,
laser->laser_pose.y, laser->laser_pose.theta,
laser->robot_pose.x, laser->robot_pose.y,
laser->robot_pose.theta);
carmen_fprintf(outfile, "%f %f %f %f %f ", laser->v, laser->phi,
laser->forward_safety_dist, laser->side_safety_dist,
laser->turn_axis);
carmen_fprintf(outfile, "%f %s %f\n", laser->timestamp,
laser->host, timestamp);
}
void carmen_logwrite_write_ultrasonic_sonar_sensor(carmen_ultrasonic_sonar_sensor_message *sonar,
carmen_FILE *outfile,
double timestamp)
{
int i;
carmen_fprintf(outfile, "ULTRASONIC_SONAR_SENSOR %d %d %lf %lf %lf %lf ", sonar->number_of_sonars, sonar->sonar_beans, sonar->fov, sonar->angle_step, sonar->start_angle, sonar->max_range);
for (i=0; i<4; i++)
carmen_fprintf(outfile, "%.2lf ", sonar->sensor[i]);
carmen_fprintf(outfile, "%lf %s %lf\n", sonar->timestamp, sonar->host, timestamp);
}
void carmen_logwrite_write_base_ackerman_motion(carmen_base_ackerman_motion_command_message *msg,
carmen_FILE *outfile,
double timestamp)
{
int i;
carmen_fprintf(outfile, "BASEMOTION_ACK %d", msg->num_motion_commands);
for(i = 0; i < msg->num_motion_commands; i++)
carmen_fprintf(outfile, " %f %f %f",
msg->motion_command[i].v, msg->motion_command[i].phi,
msg->motion_command[i].time);
carmen_fprintf(outfile, " %f %s %f\n", msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_to_file_bumblebee_basic_steroimage(carmen_bumblebee_basic_stereoimage_message* msg, int bumblebee_num, carmen_FILE *outfile,
double timestamp, int frequency, char *log_filename)
{
const double HIGH_LEVEL_SUBDIR_TIME = 100.0 * 100.0; // new each 100 x 100 seconds
const double LOW_LEVEL_SUBDIR_TIME = 100.0; // new each 100 seconds
int high_level_subdir = ((int) (msg->timestamp / HIGH_LEVEL_SUBDIR_TIME)) * HIGH_LEVEL_SUBDIR_TIME;
int low_level_subdir = ((int) (msg->timestamp / LOW_LEVEL_SUBDIR_TIME)) * LOW_LEVEL_SUBDIR_TIME;
static char directory[1024];
static char subdir[1024];
static char path[1024];
/**
* TODO: @Filipe: Check if the mkdir call is time consuming.
*/
sprintf(directory, "%s_bumblebee", log_filename);
mkdir(directory, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
sprintf(subdir, "%s/%d", directory, high_level_subdir);
mkdir(subdir, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
sprintf(subdir, "%s/%d/%d", directory, high_level_subdir, low_level_subdir);
mkdir(subdir, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
// DEBUG:
//printf("%lf %d %d %s\n", msg->timestamp, high_level_subdir, low_level_subdir, subdir);
if ((frame_number % frequency) == 0)
{
sprintf(path, "%s/%lf.bb%d.image", subdir, msg->timestamp, bumblebee_num);
FILE *image_file = fopen(path, "wb");
fwrite(msg->raw_left, msg->image_size, sizeof(unsigned char), image_file);
fwrite(msg->raw_right, msg->image_size, sizeof(unsigned char), image_file);
fclose(image_file);
carmen_fprintf(outfile, "BUMBLEBEE_BASIC_STEREOIMAGE_IN_FILE%d %s %d %d %d %d ", bumblebee_num, path,
msg->width, msg->height, msg->image_size, msg->isRectified);
carmen_fprintf(outfile, "%f %s %f\n", msg->timestamp, msg->host, timestamp);
frame_number = 0;
}
frame_number++;
}
void carmen_logwrite_write_base_ackerman_velocity(carmen_base_ackerman_velocity_message *msg,
carmen_FILE *outfile,
double timestamp)
{
carmen_fprintf(outfile, "BASEVELOCITY_ACK %f %f %f %s %f\n",
msg->v, msg->phi, msg->timestamp, msg->host, timestamp);
}
void carmen_logger_write_gps_gpgga(carmen_gps_gpgga_message *gps_msg,
carmen_FILE *outfile, double timestamp)
{
char lat_o = gps_msg->lat_orient;
char long_o = gps_msg->long_orient;
if (lat_o == '\0')
lat_o = 'N';
if (long_o == '\0')
long_o = 'E';
carmen_fprintf(outfile,"NMEAGGA %d %lf %lf %c %lf %c %d %d %lf %lf %lf %lf %lf %d %lf %s %lf\n",
gps_msg->nr,
gps_msg->utc,
gps_msg->latitude_dm,
lat_o,
gps_msg->longitude_dm,
long_o,
gps_msg->gps_quality, gps_msg->num_satellites,
gps_msg->hdop,
gps_msg->sea_level,
gps_msg->altitude,
gps_msg->geo_sea_level,
gps_msg->geo_sep,
gps_msg->data_age,
gps_msg->timestamp, gps_msg->host, timestamp);
}
void carmen_logwrite_write_sync(carmen_logger_sync_message *sync_message,
carmen_FILE *outfile)
{
carmen_fprintf(outfile, "SYNC %s %f %s %f\n", sync_message->tag,
sync_message->timestamp, sync_message->host,
carmen_get_time());
}
void carmen_logger_write_gps_gprmc(carmen_gps_gprmc_message *gps_msg,
carmen_FILE *outfile, double timestamp)
{
char lat_o = gps_msg->lat_orient;
char long_o = gps_msg->long_orient;
char vardir = gps_msg->var_dir;
if (lat_o == '\0')
lat_o = 'N';
if (long_o == '\0')
long_o = 'E';
if (vardir == '\0')
vardir = 'E';
carmen_fprintf(outfile,"NMEARMC %d %d %lf %lf %c %lf %c %lf %lf %lf %c %d %lf %s %lf\n",
gps_msg->nr,
gps_msg->validity,
gps_msg->utc,
gps_msg->latitude_dm,
lat_o,
gps_msg->longitude_dm,
long_o,
gps_msg->speed,
gps_msg->true_course,
gps_msg->variation,
vardir,
gps_msg->date,
gps_msg->timestamp, gps_msg->host, timestamp);
}
void carmen_logwrite_write_robot_ackerman_vector_move(carmen_robot_ackerman_vector_move_message *msg,
carmen_FILE *outfile,
double timestamp)
{
carmen_fprintf(outfile, "VECTORMOVE_ACK %f %f %f %s %f\n",
msg->distance, msg->theta, msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_param(char *module, char *variable, char *value,
double ipc_time, char *hostname,
carmen_FILE *outfile, double timestamp)
{
carmen_fprintf(outfile, "PARAM %s_%s %s %f %s %f\n", module,
variable, value, ipc_time, hostname, timestamp);
}
void
carmen_logwrite_write_variable_velodyne_scan(carmen_velodyne_variable_scan_message* msg, carmen_FILE* outfile, double timestamp)
{
int i, j, k, angle;
carmen_fprintf(outfile, "VARIABLE_VELODYNE_SCAN ");
carmen_fprintf(outfile, "%d ", msg->number_of_shots);
carmen_fprintf(outfile, "%d ", msg->partial_scan[0].shot_size);
int vector_size = (2 * msg->partial_scan[0].shot_size + 4 * msg->partial_scan[0].shot_size + 1);
if (hex_char_distance_and_intensity_variable == NULL)
{
hex_char_distance_and_intensity_variable = (char *) malloc(vector_size * sizeof(char)); // 2 * 32 laser intensities and 4 * 32 laser distances
for (i = 0; i < 16; i++)
{
if (i <= 9)
int_to_nibble_hex[i] = '0' + i;
else
int_to_nibble_hex[i] = 'a' + i - 10;
}
}
for(i = 0; i < msg->number_of_shots; i++)
{
angle = (int)(msg->partial_scan[i].angle * 100);
carmen_fprintf(outfile, "%d ", angle);
for(j = k = 0; j < msg->partial_scan[0].shot_size; j += 1, k += 6)
{
hex_char_distance_and_intensity_variable[k] = GET_SHORT_FIRST_NIBBLE(msg->partial_scan[i].distance[j]);
hex_char_distance_and_intensity_variable[k + 1] = GET_SHORT_SECOND_NIBBLE(msg->partial_scan[i].distance[j]);
hex_char_distance_and_intensity_variable[k + 2] = GET_SHORT_THIRD_NIBBLE(msg->partial_scan[i].distance[j]);
hex_char_distance_and_intensity_variable[k + 3] = GET_SHORT_FOURTH_NIBBLE(msg->partial_scan[i].distance[j]);
hex_char_distance_and_intensity_variable[k + 4] = GET_LOW_ORDER_NIBBLE(msg->partial_scan[i].intensity[j]);
hex_char_distance_and_intensity_variable[k + 5] = GET_HIGH_ORDER_NIBBLE(msg->partial_scan[i].intensity[j]);
}
hex_char_distance_and_intensity_variable[k] = ' ';
carmen_fwrite(hex_char_distance_and_intensity_variable, vector_size, 1, outfile);
}
carmen_fprintf(outfile, "%f %s %f\n", msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_visual_odometry(carmen_visual_odometry_pose6d_message *odometry,
carmen_FILE *outfile, double timestamp)
{
carmen_fprintf(outfile, "VISUAL_ODOMETRY %f %f %f %f %f %f %f %f %f %s %f\n",
odometry->pose_6d.x, odometry->pose_6d.y, odometry->pose_6d.z,
odometry->pose_6d.roll, odometry->pose_6d.pitch, odometry->pose_6d.yaw,
odometry->v, odometry->phi, odometry->timestamp, odometry->host,
timestamp);
}
void carmen_logwrite_write_robot_ackerman_follow_trajectory(carmen_robot_ackerman_follow_trajectory_message *msg,
carmen_FILE *outfile,
double timestamp)
{
carmen_fprintf(outfile, "FOLLOWTRAJECTORY_ACK %lf %lf %lf %lf %lf ",
msg->robot_position.x, msg->robot_position.y, msg->robot_position.theta, msg->robot_position.v, msg->robot_position.phi);
carmen_fprintf(outfile, "%d ", msg->trajectory_length);
int i;
for (i=0; i<msg->trajectory_length; i++)
carmen_fprintf(outfile, "%lf %lf %lf %lf %lf ",
msg->trajectory[i].x, msg->trajectory[i].y, msg->trajectory[i].theta, msg->trajectory[i].v, msg->trajectory[i].phi);
carmen_fprintf(outfile, "%f %s %f\n",
msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_odometry_ackerman(carmen_base_ackerman_odometry_message *odometry,
carmen_FILE *outfile,
double timestamp)
{
carmen_fprintf(outfile, "ODOM_ACK %f %f %f %f %f %f %s %f\n", odometry->x,
odometry->y, odometry->theta, odometry->v, odometry->phi,
odometry->timestamp, odometry->host,
timestamp);
}
void carmen_logwrite_write_localize_ackerman(carmen_localize_ackerman_globalpos_message *msg,
carmen_FILE *outfile, double timestamp)
{
carmen_fprintf(outfile, "GLOBALPOS_ACK %f %f %f %f %f %f %f %f %f %f %f %f %d %f %s %f\n",
msg->globalpos.x, msg->globalpos.y, msg->globalpos.theta,
msg->globalpos_std.x, msg->globalpos_std.y, msg->globalpos_std.theta,
msg->odometrypos.x, msg->odometrypos.y, msg->odometrypos.theta,
msg->v, msg->phi, msg->globalpos_xy_cov, msg->converged,
msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_ackerman_truepos(carmen_simulator_ackerman_truepos_message *truepos,
carmen_FILE *outfile, double timestamp)
{
carmen_fprintf(outfile, "TRUEPOS_ACK %f %f %f %f %f %f %f %s %f\n",
truepos->truepose.x, truepos->truepose.y,
truepos->truepose.theta, truepos->odometrypose.x,
truepos->odometrypose.y, truepos->odometrypose.theta,
truepos->timestamp, truepos->host, timestamp);
}
void nsick_writelog_write_header(carmen_FILE *outfile) {
carmen_fprintf(outfile, "%s\n", CARMEN_LOGFILE_HEADER);
carmen_fprintf(outfile, "# file format is one message per line\n");
carmen_fprintf(outfile, "# message_name [message contents] ipc_timestamp ipc_hostname logger_timestamp\n");
carmen_fprintf(outfile, "# message formats defined: NSICKSTATUS NSICKLASERPOS NSICKLASER1\n");
carmen_fprintf(outfile, "# NSICKSTATUS pos\n");
carmen_fprintf(outfile, "# NSICKLASERPOS laser_id x y z yaw pitch roll\n");
carmen_fprintf(outfile, "# NSICKLASER1 laser_type start_angle field_of_view angular_resolution maximum_range accuracy remission_mode num_readings [range_readings] num_remissions [remission values]\n");
}
void carmen_logwrite_write_to_file_velodyne(carmen_velodyne_partial_scan_message* msg, carmen_FILE *outfile,
double timestamp, char *log_filename)
{
const double HIGH_LEVEL_SUBDIR_TIME = 100.0 * 100.0; // new each 100 x 100 seconds
const double LOW_LEVEL_SUBDIR_TIME = 100.0; // new each 100 seconds
int high_level_subdir = ((int) (msg->timestamp / HIGH_LEVEL_SUBDIR_TIME)) * HIGH_LEVEL_SUBDIR_TIME;
int low_level_subdir = ((int) (msg->timestamp / LOW_LEVEL_SUBDIR_TIME)) * LOW_LEVEL_SUBDIR_TIME;
int i;
static char directory[1024];
static char subdir[1024];
static char path[1024];
/**
* TODO: @Filipe: Check if the mkdir call is time consuming.
*/
sprintf(directory, "%s_velodyne", log_filename);
mkdir(directory, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
sprintf(subdir, "%s/%d", directory, high_level_subdir);
mkdir(subdir, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
sprintf(subdir, "%s/%d/%d", directory, high_level_subdir, low_level_subdir);
mkdir(subdir, ACCESSPERMS); // if the directory exists, mkdir returns an error silently
sprintf(path, "%s/%lf.pointcloud", subdir, msg->timestamp);
FILE *image_file = fopen(path, "wb");
for(i = 0; i < msg->number_of_32_laser_shots; i++)
{
fwrite(&(msg->partial_scan[i].angle), sizeof(double), 1, image_file);
fwrite(msg->partial_scan[i].distance, sizeof(short), 32, image_file);
fwrite(msg->partial_scan[i].intensity, sizeof(char), 32, image_file);
}
fclose(image_file);
carmen_fprintf(outfile, "VELODYNE_PARTIAL_SCAN_IN_FILE %s %d ", path, msg->number_of_32_laser_shots);
carmen_fprintf(outfile, "%f %s %f\n", msg->timestamp, msg->host, timestamp);
}
void carmen_logwrite_write_laser_ldmrs_objects_data(carmen_laser_ldmrs_objects_data_message *laser,
int laser_num, carmen_FILE *outfile,
double timestamp)
{
int i;
(void)laser_num;
carmen_fprintf(outfile, "LASER_LDMRS_OBJECTS_DATA ");
carmen_fprintf(outfile, "%d ",
laser->num_objects);
for(i = 0; i < laser->num_objects; i++)
{
carmen_fprintf(outfile,"%d %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d ",
laser->objects_data_list[i].object_id,
laser->objects_data_list[i].object_age,
laser->objects_data_list[i].object_prediction_age,
laser->objects_data_list[i].reference_point_x,
laser->objects_data_list[i].reference_point_y,
laser->objects_data_list[i].reference_point_sigma_x,
laser->objects_data_list[i].reference_point_sigma_y,
laser->objects_data_list[i].closest_point_x,
laser->objects_data_list[i].closest_point_y,
laser->objects_data_list[i].bounding_box_center_x,
laser->objects_data_list[i].bounding_box_center_y,
laser->objects_data_list[i].bounding_box_length,
laser->objects_data_list[i].bounding_box_width,
laser->objects_data_list[i].object_box_center_x,
laser->objects_data_list[i].object_box_center_y,
laser->objects_data_list[i].object_box_lenght,
laser->objects_data_list[i].object_box_width,
laser->objects_data_list[i].object_box_orientation,
laser->objects_data_list[i].abs_velocity_x,
laser->objects_data_list[i].abs_velocity_y,
laser->objects_data_list[i].abs_velocity_sigma_x,
laser->objects_data_list[i].abs_velocity_sigma_y,
laser->objects_data_list[i].relative_velocity_x,
laser->objects_data_list[i].relative_velocity_y,
laser->objects_data_list[i].class_id);
}
carmen_fprintf(outfile, "%f %s %f\n", laser->timestamp,
laser->host, timestamp);
}
void carmen_logger_write_gps_gphdt(carmen_gps_gphdt_message *gps_msg,
carmen_FILE *outfile, double timestamp)
{
carmen_fprintf(outfile,"NMEAHDT %d %lf %d %lf %s %lf\n",
gps_msg->nr,
gps_msg->heading,
gps_msg->valid,
gps_msg->timestamp,
gps_msg->host,
timestamp);
}
void carmen_logwrite_write_kinect_depth(carmen_kinect_depth_message *kinect,
int kinect_num, carmen_FILE *outfile,
double timestamp)
{
int i, j;
unsigned short depth_value;
carmen_fprintf(outfile, "RAW_KINECT_DEPTH%d ", kinect_num);
carmen_fprintf(outfile, "%d ", kinect->width);
carmen_fprintf(outfile, "%d ", kinect->height);
carmen_fprintf(outfile, "%d ", kinect->size);
if (hex_char_depth_kinect == NULL)
{
hex_char_depth_kinect = (char *) malloc((4 * kinect->size) * sizeof(char));
for (i=0; i < 16; i++)
{
if (i <= 9)
int_to_nibble_hex[i] = '0' + i;
else
int_to_nibble_hex[i] = 'a' + i - 10;
}
}
for(i=j=0; i<(kinect->size); i++, j+=4)
{
depth_value = convert_kinect_depth_meters_to_raw(kinect->depth[i]);
hex_char_depth_kinect[j] = GET_SHORT_FIRST_NIBBLE(depth_value);
hex_char_depth_kinect[j+1] = GET_SHORT_SECOND_NIBBLE(depth_value);
hex_char_depth_kinect[j+2] = GET_SHORT_THIRD_NIBBLE(depth_value);
hex_char_depth_kinect[j+3] = GET_SHORT_FOURTH_NIBBLE(depth_value);
}
carmen_fwrite(hex_char_depth_kinect, (4 * kinect->size), 1, outfile);
carmen_fprintf(outfile, "%f %s %f\n", kinect->timestamp, kinect->host, timestamp);
}
void carmen_logwrite_write_logger_comment(carmen_logger_comment_message *msg,
carmen_FILE *outfile,
double timestamp)
{
unsigned int l = strlen(msg->text);
char buffer[l+1];
strcpy( buffer, msg->text );
unsigned int x;
for (x=0; x<=l; x++)
{
if (msg->text[x]==' ') msg->text[x]='_';
}
carmen_fprintf(outfile, "COMMENT %s %f %s %f\n",
msg->text, msg->timestamp, msg->host, timestamp);
}
void robot_frontlaser_handler(carmen_robot_laser_message *frontlaser)
{
int i;
double x, y;
if (localize_msg->timestamp == 0)
return;
fprintf(stderr, "F");
carmen_localize_correct_laser(frontlaser, localize_msg);
for(i = 0; i < frontlaser->num_readings; i++) {
x = frontlaser->laser_pose.x+cos(-M_PI/2 + M_PI*i/180.0 + frontlaser->laser_pose.theta)*frontlaser->range[i];
y = frontlaser->laser_pose.y+sin(-M_PI/2 + M_PI*i/180.0 + frontlaser->laser_pose.theta)*frontlaser->range[i];
carmen_fprintf(outfile, "%.2f %.2f\n", x, y);
}
}
void carmen_logwrite_write_bumblebee_basic_steroimage_old(carmen_bumblebee_basic_stereoimage_message* msg, int bumblebee_num, carmen_FILE *outfile,
double timestamp)
{
int i;
carmen_fprintf(outfile, "BUMBLEBEE_BASIC_STEREOIMAGE%d ", bumblebee_num);
carmen_fprintf(outfile, "%d ", msg->width);
carmen_fprintf(outfile, "%d ", msg->height);
carmen_fprintf(outfile, "%d ", msg->image_size);
carmen_fprintf(outfile, "%d ", msg->isRectified);
for(i=0; i<(msg->image_size); i++)
carmen_fprintf(outfile, "%d ", (int)msg->raw_right[i]);
for(i=0; i<(msg->image_size); i++)
carmen_fprintf(outfile, "%d ", (int)msg->raw_left[i]);
carmen_fprintf(outfile, "%f %s %f\n", msg->timestamp, msg->host, timestamp);
}
