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_bumblebee_basic_steroimage(carmen_bumblebee_basic_stereoimage_message* msg, int bumblebee_num, carmen_FILE *outfile,
double timestamp, int frequency)
{
int i, j;
if ((frame_number % frequency ) == 0)
{
if (hex_char_image == NULL)
{
hex_char_image = (char *) malloc((2 * msg->image_size + 1) * sizeof(char)); // Twice the number of bytes plus 1 for a space at the end
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;
}
}
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=j=0; i<(msg->image_size); i++, j+=2)
{
hex_char_image[j] = GET_HIGH_ORDER_NIBBLE(msg->raw_right[i]);
hex_char_image[j+1] = GET_LOW_ORDER_NIBBLE(msg->raw_right[i]);
}
hex_char_image[j] = ' ';
carmen_fwrite(hex_char_image, 2 * msg->image_size + 1 , 1, outfile);
for(i=j=0; i<(msg->image_size); i++, j+=2)
{
hex_char_image[j] = GET_HIGH_ORDER_NIBBLE(msg->raw_left[i]);
hex_char_image[j+1] = GET_LOW_ORDER_NIBBLE(msg->raw_left[i]);
}
hex_char_image[j] = ' ';
carmen_fwrite(hex_char_image, 2 * msg->image_size + 1, 1, outfile);
carmen_fprintf(outfile, "%f %s %f\n", msg->timestamp, msg->host, timestamp);
frame_number = 0;
}
frame_number++;
}
void write_color_glv(carmen_FILE *fp,
unsigned char r, unsigned char g, unsigned char b) {
unsigned char message[10];
message[0] = MESSAGE_ID_COLOR;
message[1] = r;
message[2] = g;
message[3] = b;
carmen_fwrite(message, 4, 1, fp);
}
void write_point_glv(carmen_FILE *fp,
float x, float y, float z) {
char message[20];
float *fmessage;
message[0] = MESSAGE_ID_POINT;
fmessage = (float *)(message + 1);
fmessage[0] = x;
fmessage[1] = y;
fmessage[2] = z;
carmen_fwrite(message, 13, 1, fp);
}
void write_line_glv(carmen_FILE *fp,
float x1, float y1, float z1,
float x2, float y2, float z2) {
char message[30];
float *fmessage;
message[0] = MESSAGE_ID_LINE;
fmessage = (float *)(message + 1);
fmessage[0] = x1;
fmessage[1] = y1;
fmessage[2] = z1;
fmessage[3] = x2;
fmessage[4] = y2;
fmessage[5] = z2;
carmen_fwrite(message, 25, 1, fp);
}
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 write_face_glv(carmen_FILE *fp,
float x1, float y1, float z1,
float x2, float y2, float z2,
float x3, float y3, float z3) {
char message[40];
float *fmessage;
message[0] = MESSAGE_ID_FACE;
fmessage = (float *)(message + 1);
fmessage[0] = x1;
fmessage[1] = y1;
fmessage[2] = z1;
fmessage[3] = x2;
fmessage[4] = y2;
fmessage[5] = z2;
fmessage[6] = x3;
fmessage[7] = y3;
fmessage[8] = z3;
carmen_fwrite(message, 37, 1, fp);
}
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);
}
