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