kinect/codes/Azure-Kinect-Sensor-SDK/tests/TestUtil/k4aCmd.c

457 lines
13 KiB
C
Raw Normal View History

2024-03-06 18:05:53 +00:00
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
//************************ Includes *****************************
#include <k4a/k4a.h>
#include <k4ainternal/usbcommand.h>
#include "Cli.h"
#include "Main.h"
#include "k4aCmd.h"
//**************Symbolic Constant Macros (defines) *************
#define CLI_MENU_K4A "k4a"
#define MAX_BUFFER_SIZE 256
#define TIMESTAMP_CONVERSION 1000000
#define ERROR_COUNT_MAX 10
#define FILE_FORMAT_BINARY "wb"
#define FILE_FORMAT_ASCII "w"
//************************ Typedefs *****************************
//************ Declarations (Statics and globals) ***************
static uint8_t data_buffer[MAX_BUFFER_SIZE];
//******************* Function Prototypes ***********************
//*********************** Functions *****************************
/**
* Command to get the serial numbers
*
* @param Argc Number of variables handed in
*
* @param Argv Pointer to variable array
*
* @return
* CLI_SUCCESS If executed properly
* CLI_ERROR If error was detected
*
*/
static CLI_STATUS k4a_serial_num(int Argc, char **Argv)
{
(void)Argc; // unused
(void)Argv; // unused
k4a_device_t handle;
CLI_STATUS status = CLI_SUCCESS;
handle = get_k4a_handle(0);
if (handle != NULL)
{
size_t serial_number_size = MAX_BUFFER_SIZE;
if (k4a_device_get_serialnum(handle, (char *)data_buffer, &serial_number_size) == K4A_BUFFER_RESULT_SUCCEEDED)
{
// display the serial number data
printf("SerialNumber: ");
for (uint8_t loop = 0; loop < 64; loop += 2)
{
if (loop % 32 == 0)
printf("\n ");
uint16_t data = 0;
memcpy(&data, &data_buffer[loop], sizeof(data));
printf("%04X ", data);
}
printf("\n");
}
else
{
printf("Couldn't read from device\n");
status = CLI_ERROR;
}
}
else
{
printf("Device not found\n");
status = CLI_ERROR;
}
return status;
}
/**
* Command to get the version information
*
* @param Argc Number of variables handed in
*
* @param Argv Pointer to variable array
*
* @return
* CLI_SUCCESS If executed properly
* CLI_ERROR If error was detected
*
*/
static CLI_STATUS k4a_version(int Argc, char **Argv)
{
(void)Argc; // unused
(void)Argv; // unused
k4a_device_t handle;
CLI_STATUS status = CLI_SUCCESS;
k4a_hardware_version_t version;
handle = get_k4a_handle(0);
if (handle != NULL)
{
if (k4a_device_get_version(handle, &version) == K4A_RESULT_SUCCEEDED)
{
// display the version
printf("RGB version: %d.%d.%d\n", version.rgb.major, version.rgb.minor, version.rgb.iteration);
printf("Depth version: %d.%d.%d\n", version.depth.major, version.depth.minor, version.depth.iteration);
printf("Audio version: %d.%d.%d\n", version.audio.major, version.audio.minor, version.audio.iteration);
printf("Depth Sequence version: %d.%d\n", version.depth_sensor.major, version.depth_sensor.minor);
printf("%s\n", version.firmware_build == K4A_FIRMWARE_BUILD_RELEASE ? "Release" : "Debug");
if (version.firmware_signature == K4A_FIRMWARE_SIGNATURE_MSFT)
{
printf("MSFT\n");
}
if (version.firmware_signature == K4A_FIRMWARE_SIGNATURE_TEST)
{
printf("TEST\n");
}
if (version.firmware_signature == K4A_FIRMWARE_SIGNATURE_UNSIGNED)
{
printf("No Signature\n");
}
}
else
{
printf("Couldn't read from device\n");
status = CLI_ERROR;
}
}
else
{
printf("Device not found\n");
status = CLI_ERROR;
}
return status;
}
/**
* Command to record a number of depth frames to disk
*
* @param Argc Number of variables handed in
*
* @param Argv Pointer to variable array
*
* @return
* CLI_SUCCESS If executed properly
* CLI_ERROR If error was detected
*
*/
static CLI_STATUS k4a_record_depth(int Argc, char **Argv)
{
CLI_STATUS status = CLI_SUCCESS;
uint32_t device_num = 0;
uint32_t stream_count = 0;
uint32_t mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
uint32_t fps = K4A_FRAMES_PER_SECOND_30;
char *file_name = "depth.rec";
k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
int32_t timeout_ms = 70;
k4a_capture_t capture;
k4a_device_t device = NULL;
uint32_t wait_error_count = 0;
FILE *p_file;
if (Argc < 3)
{
CliDisplayUsage(k4a_record_depth);
return CLI_ERROR;
}
// Get device number
if (!CliGetStrVal(Argv[1], &device_num))
{
return CLI_ERROR;
}
// Get number of frames to read
if (!CliGetStrVal(Argv[2], &stream_count))
{
return CLI_ERROR;
}
// Get optional operating mode
if ((Argc > 3) && ((!CliGetStrVal(Argv[3], &mode)) || (mode > K4A_DEPTH_MODE_PASSIVE_IR)))
{
return CLI_ERROR;
}
// Get optional FPS
if ((Argc > 4) &&
((!CliGetStrVal(Argv[4], &fps)) ||
(fps != K4A_FRAMES_PER_SECOND_5 && fps != K4A_FRAMES_PER_SECOND_15 && fps != K4A_FRAMES_PER_SECOND_30)))
{
return CLI_ERROR;
}
device = get_k4a_handle(device_num);
// open a file to write to
#ifdef _WIN32
if (fopen_s(&p_file, file_name, FILE_FORMAT_BINARY) == 0)
{
#else
if ((p_file = fopen(file_name, FILE_FORMAT_BINARY)) != NULL)
{
#endif
// start the stream
config.color_format = K4A_IMAGE_FORMAT_COLOR_MJPG;
config.color_resolution = K4A_COLOR_RESOLUTION_OFF;
config.depth_mode = mode;
config.camera_fps = fps;
// start streaming.
if (K4A_FAILED(k4a_device_start_cameras(device, &config)))
{
return CLI_ERROR;
}
// loop through stream data
while (stream_count-- > 0)
{
// Synchronize
if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device, &capture, timeout_ms))
{
wait_error_count = 0; // reset the error count
uint8_t *buffer;
k4a_image_t image;
image = k4a_capture_get_depth_image(capture);
if (image)
{
buffer = k4a_image_get_buffer(image);
if (buffer)
{
// store to disk
fwrite(buffer, 1, k4a_image_get_size(image), p_file);
}
k4a_image_release(image);
}
image = k4a_capture_get_ir_image(capture);
if (image)
{
buffer = k4a_image_get_buffer(image);
if (buffer)
{
// store to disk
fwrite(buffer, 1, k4a_image_get_size(image), p_file);
}
}
k4a_capture_release(capture);
}
else
{
wait_error_count++;
if (wait_error_count > ERROR_COUNT_MAX)
{
printf("Failed to receive a frame within %d mSec\n", timeout_ms * ERROR_COUNT_MAX);
status = CLI_ERROR;
break;
}
}
};
k4a_device_stop_cameras(device);
fclose(p_file);
}
else
{
printf("Could not open %s to write\n", file_name);
status = CLI_ERROR;
}
return status;
}
/**
* Command to record IMU samples
*
* @param Argc Number of variables handed in
*
* @param Argv Pointer to variable array
*
* @return
* CLI_SUCCESS If executed properly
* CLI_ERROR If error was detected
*
*/
static CLI_STATUS k4a_record_imu(int Argc, char **Argv)
{
CLI_STATUS status = CLI_SUCCESS;
uint32_t stream_count;
uint32_t device_num = 0;
bool csv = false;
FILE *p_file = 0;
k4a_device_t device = NULL;
k4a_wait_result_t result;
char *file_name = "imu.rec";
uint32_t wait_error_count = 0;
int32_t timeout_ms = 150;
k4a_imu_sample_t imu_sample;
char *file_format = FILE_FORMAT_BINARY;
if (Argc < 3)
{
CliDisplayUsage(k4a_record_imu);
return CLI_ERROR;
}
// Get device number
if (!CliGetStrVal(Argv[1], &device_num))
{
return CLI_ERROR;
}
// Get number of samples to record
if (!CliGetStrVal(Argv[2], &stream_count))
{
return CLI_ERROR;
}
// Check if CSV option is selected
if ((Argc > 3) && (memcmp(Argv[3], "csv", 3) == 0))
{
csv = true;
file_format = FILE_FORMAT_ASCII;
file_name = "imu.csv";
}
device = get_k4a_handle(device_num);
// open a file to write to
#ifdef _WIN32
if (fopen_s(&p_file, file_name, file_format) == 0)
{
#else
if ((p_file = fopen(file_name, file_format)) != NULL)
{
#endif
// configure and start stream
if (k4a_device_start_imu(device) == K4A_RESULT_SUCCEEDED)
{
// loop through stream data
if (csv)
{
// Print csv header
fprintf(p_file, "Temp, Accel TS, Accel X, Accel Y, Accel Z, Gyro TS, Gyro X, Gyro Y, Gyro Z\n");
}
while (stream_count-- > 0)
{
result = k4a_device_get_imu_sample(device, &imu_sample, timeout_ms);
if (result == K4A_WAIT_RESULT_TIMEOUT)
{
wait_error_count++;
if (wait_error_count > ERROR_COUNT_MAX)
{
printf("Failed to receive a frame within %d mSec\n", timeout_ms * ERROR_COUNT_MAX);
status = CLI_ERROR;
break;
}
}
else if (result == K4A_WAIT_RESULT_SUCCEEDED)
{
wait_error_count = 0; // reset the error count
// store to disk
if (csv)
{
fprintf(p_file,
"%f, %f, %f, %f, %f, %f, %f, %f, %f\n",
(double)imu_sample.temperature,
(double)imu_sample.acc_timestamp_usec / TIMESTAMP_CONVERSION,
(double)imu_sample.acc_sample.xyz.x,
(double)imu_sample.acc_sample.xyz.y,
(double)imu_sample.acc_sample.xyz.z,
(double)imu_sample.gyro_timestamp_usec / TIMESTAMP_CONVERSION,
(double)imu_sample.gyro_sample.xyz.x,
(double)imu_sample.gyro_sample.xyz.y,
(double)imu_sample.gyro_sample.xyz.z);
}
else
{
fwrite(&imu_sample, 1, sizeof(imu_sample), p_file);
}
}
};
}
else
{
printf("Couldn't start the stream\n");
status = CLI_ERROR;
}
k4a_device_stop_imu(device);
fclose(p_file);
}
else
{
printf("Could not open %s to write\n", file_name);
status = CLI_ERROR;
}
return status;
}
/**
* initialization for the K4A command interface
*
*/
void k4a_cmd_init(void)
{
CliRegister(CLI_MENU_K4A,
"serialnum",
k4a_serial_num,
"Display the serial numbers",
"Display the serial numbers\n"
"Syntax: serialnum\n"
"Example: serialnum\n");
CliRegister(CLI_MAIN_MENU,
"version",
k4a_version,
"Display version information",
"Display version information\n"
"Syntax: version\n"
"Example: version\n");
CliRegister(CLI_MENU_K4A,
"recdepth",
k4a_record_depth,
"Record depth frames",
"Store to disk a set number of frames\n"
"Syntax: recdepth <device> <# of frames> [mode] [FPS]\n"
"Example: recdepth 0 60 1 1\n"
"Acceptable modes (Default NFOV_UNBINNED:\n"
" NFOV_2x2BINNED = 0\n"
" NFOV_UNBINNED = 1\n"
" WFOV_2x2BINNED = 2\n"
" WFOV_UNBINNED = 3\n"
" PASSIVE_IR = 4\n"
"Acceptable FPS (Default 30 FPS, depends on mode):\n"
" 30 FPS = 0\n"
" 15 FPS = 1\n"
" 5 FPS = 2\n");
CliRegister(CLI_MENU_K4A,
"recimu",
k4a_record_imu,
"Record IMU stream",
"Store to disk IMU set number of samples\n"
"Syntax: recimu <device> <# of samples>\n"
"Example: recimu 0 100\n");
}