306 lines
9.2 KiB
C
306 lines
9.2 KiB
C
#include <zmq.h>
|
|
#include <cjson/cJSON.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <uuid/uuid.h>
|
|
#include <stdbool.h>
|
|
|
|
#define MAX_MSG_SIZE 4096
|
|
#define LOG_INFO(...) printf("[INFO] " __VA_ARGS__)
|
|
#define LOG_ERROR(...) fprintf(stderr, "[ERROR] " __VA_ARGS__)
|
|
#define LOG_WARNING(...) fprintf(stderr, "[WARNING] " __VA_ARGS__)
|
|
#define LOG_DEBUG(...) printf("[DEBUG] " __VA_ARGS__)
|
|
|
|
typedef struct {
|
|
void* context;
|
|
void* socket;
|
|
} VTXClient;
|
|
|
|
// Function to generate a unique request ID
|
|
static char* get_request_id() {
|
|
static char request_id[50];
|
|
time_t now;
|
|
struct tm *tm_info;
|
|
uuid_t uuid;
|
|
char uuid_str[37];
|
|
|
|
time(&now);
|
|
tm_info = localtime(&now);
|
|
uuid_generate(uuid);
|
|
uuid_unparse_lower(uuid, uuid_str);
|
|
|
|
strftime(request_id, 20, "%Y%m%d%H%M%S", tm_info);
|
|
strcat(request_id, "-");
|
|
strncat(request_id, uuid_str, 8);
|
|
|
|
return request_id;
|
|
}
|
|
|
|
// Initialize the VTXClient
|
|
VTXClient* vtx_client_init() {
|
|
VTXClient* client = (VTXClient*)malloc(sizeof(VTXClient));
|
|
if (!client) {
|
|
LOG_ERROR("Failed to allocate memory for client\n");
|
|
return NULL;
|
|
}
|
|
|
|
client->context = zmq_ctx_new();
|
|
if (!client->context) {
|
|
LOG_ERROR("Failed to create ZMQ context\n");
|
|
free(client);
|
|
return NULL;
|
|
}
|
|
|
|
client->socket = zmq_socket(client->context, ZMQ_DEALER);
|
|
if (!client->socket) {
|
|
LOG_ERROR("Failed to create ZMQ socket\n");
|
|
zmq_ctx_destroy(client->context);
|
|
free(client);
|
|
return NULL;
|
|
}
|
|
|
|
if (zmq_connect(client->socket, "ipc:///tmp/5555") != 0) {
|
|
LOG_ERROR("Failed to connect to server\n");
|
|
zmq_close(client->socket);
|
|
zmq_ctx_destroy(client->context);
|
|
free(client);
|
|
return NULL;
|
|
}
|
|
|
|
int timeout = 200;
|
|
zmq_setsockopt(client->socket, ZMQ_SNDTIMEO, &timeout, sizeof(timeout));
|
|
zmq_setsockopt(client->socket, ZMQ_RCVTIMEO, &timeout, sizeof(timeout));
|
|
|
|
return client;
|
|
}
|
|
|
|
// Send request and receive response
|
|
static cJSON* send_request(VTXClient* client, const char* table, const char* method, cJSON* params) {
|
|
cJSON* result = NULL;
|
|
char* request_str = NULL;
|
|
cJSON* request = NULL;
|
|
cJSON* response = NULL;
|
|
|
|
if (!client || !table || !method || !params) {
|
|
LOG_ERROR("Invalid parameters in send_request\n");
|
|
return NULL;
|
|
}
|
|
|
|
request = cJSON_CreateObject();
|
|
if (!request) {
|
|
LOG_ERROR("Failed to create request JSON object\n");
|
|
goto cleanup;
|
|
}
|
|
|
|
cJSON_AddStringToObject(request, "table", table);
|
|
cJSON_AddStringToObject(request, "method", method);
|
|
cJSON_AddItemToObject(request, "params", params);
|
|
cJSON_AddStringToObject(request, "id", get_request_id());
|
|
|
|
request_str = cJSON_PrintUnformatted(request);
|
|
if (!request_str) {
|
|
LOG_ERROR("Failed to serialize request\n");
|
|
goto cleanup;
|
|
}
|
|
|
|
LOG_DEBUG("Sending request: %s\n", request_str);
|
|
|
|
if (zmq_send(client->socket, request_str, strlen(request_str), 0) == -1) {
|
|
LOG_ERROR("Failed to send request\n");
|
|
goto cleanup;
|
|
}
|
|
|
|
char response_str[MAX_MSG_SIZE] = {0};
|
|
if (zmq_recv(client->socket, response_str, MAX_MSG_SIZE - 1, 0) == -1) {
|
|
LOG_ERROR("Failed to receive response\n");
|
|
goto cleanup;
|
|
}
|
|
|
|
response = cJSON_Parse(response_str);
|
|
if (!response) {
|
|
LOG_ERROR("Failed to parse response\n");
|
|
goto cleanup;
|
|
}
|
|
|
|
cJSON* error = cJSON_GetObjectItem(response, "error");
|
|
if (error) {
|
|
cJSON* error_msg = cJSON_GetObjectItem(error, "message");
|
|
if (error_msg && error_msg->valuestring) {
|
|
LOG_ERROR("Request failed: %s\n", error_msg->valuestring);
|
|
}
|
|
goto cleanup;
|
|
}
|
|
|
|
result = cJSON_DetachItemFromObject(response, "result");
|
|
|
|
cleanup:
|
|
if (request_str) free(request_str);
|
|
if (request) cJSON_Delete(request);
|
|
if (response) cJSON_Delete(response);
|
|
return result;
|
|
}
|
|
|
|
// Set key-value pair
|
|
void vtx_client_set(VTXClient* client, const char* table, const char* key, const char* value) {
|
|
if (!client || !table || !key || !value) {
|
|
LOG_ERROR("Invalid parameters in vtx_client_set\n");
|
|
return;
|
|
}
|
|
|
|
cJSON* params = cJSON_CreateObject();
|
|
if (!params) {
|
|
LOG_ERROR("Failed to create params JSON object\n");
|
|
return;
|
|
}
|
|
|
|
cJSON_AddStringToObject(params, "key", key);
|
|
cJSON_AddStringToObject(params, "value", value);
|
|
|
|
cJSON* result = send_request(client, table, "set", params);
|
|
if (!result) {
|
|
LOG_WARNING("Failed to set %s in %s\n", key, table);
|
|
}
|
|
|
|
if (result) cJSON_Delete(result);
|
|
// Note: params is deleted by send_request
|
|
}
|
|
|
|
// Get value by key
|
|
char* vtx_client_get(VTXClient* client, const char* table, const char* key) {
|
|
if (!client || !table || !key) {
|
|
LOG_ERROR("Invalid parameters in vtx_client_get\n");
|
|
return NULL;
|
|
}
|
|
|
|
cJSON* params = cJSON_CreateObject();
|
|
if (!params) {
|
|
LOG_ERROR("Failed to create params JSON object\n");
|
|
return NULL;
|
|
}
|
|
|
|
cJSON_AddStringToObject(params, "key", key);
|
|
|
|
cJSON* result = send_request(client, table, "get", params);
|
|
if (!result) {
|
|
LOG_WARNING("Failed to get %s from %s\n", key, table);
|
|
return NULL;
|
|
}
|
|
|
|
char* ret_val = NULL;
|
|
if (strlen(key) == 0) {
|
|
// If key is empty, return the entire result as a string
|
|
ret_val = cJSON_Print(result);
|
|
} else {
|
|
// Otherwise, get the value field
|
|
cJSON* value = cJSON_GetObjectItem(result, "value");
|
|
if (value && value->valuestring) {
|
|
ret_val = strdup(value->valuestring);
|
|
}
|
|
}
|
|
|
|
cJSON_Delete(result);
|
|
return ret_val;
|
|
}
|
|
|
|
// Delete key
|
|
void vtx_client_delete(VTXClient* client, const char* table, const char* key) {
|
|
if (!client || !table || !key) {
|
|
LOG_ERROR("Invalid parameters in vtx_client_delete\n");
|
|
return;
|
|
}
|
|
|
|
cJSON* params = cJSON_CreateObject();
|
|
if (!params) {
|
|
LOG_ERROR("Failed to create params JSON object\n");
|
|
return;
|
|
}
|
|
|
|
cJSON_AddStringToObject(params, "key", key);
|
|
|
|
cJSON* result = send_request(client, table, "delete", params);
|
|
if (!result) {
|
|
LOG_WARNING("Failed to delete %s from %s\n", key, table);
|
|
}
|
|
|
|
if (result) cJSON_Delete(result);
|
|
}
|
|
|
|
// Close client
|
|
void vtx_client_close(VTXClient* client) {
|
|
if (!client) return;
|
|
|
|
if (client->socket) {
|
|
zmq_close(client->socket);
|
|
LOG_INFO("Socket closed successfully\n");
|
|
}
|
|
if (client->context) {
|
|
zmq_ctx_destroy(client->context);
|
|
LOG_INFO("Context terminated successfully\n");
|
|
}
|
|
free(client);
|
|
}
|
|
|
|
// Example usage
|
|
// int main() {
|
|
// VTXClient* vtxdb = vtx_client_init();
|
|
// if (!vtxdb) {
|
|
// LOG_ERROR("Failed to initialize VTXClient\n");
|
|
// return 1;
|
|
// }
|
|
|
|
// // Get all configurations
|
|
// char* all_config = vtx_client_get(vtxdb, "robot_config", "");
|
|
// if (all_config) {
|
|
// printf("%s\n", all_config);
|
|
// free(all_config);
|
|
// }
|
|
|
|
// // Set robot basic configurations
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.info.name", "VTX-1000");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.info.model", "Industrial-X");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.info.serial_number", "VTX2024001");
|
|
|
|
// // Set motion parameters
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.motion.max_speed", "100");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.motion.acceleration", "2.5");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.motion.deceleration", "3.0");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.motion.joint_speed_limit", "80");
|
|
|
|
// // Set safety parameters
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.safety.collision_threshold", "50");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.safety.protective_stop_distance", "0.5");
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.safety.reduced_mode_speed", "30");
|
|
|
|
// // Read and display some configurations
|
|
// char* robot_name = vtx_client_get(vtxdb, "robot_config", "temp_config.info.name");
|
|
// char* max_speed = vtx_client_get(vtxdb, "robot_config", "temp_config.motion.max_speed");
|
|
// char* safety_threshold = vtx_client_get(vtxdb, "robot_config", "temp_config.safety.collision_threshold");
|
|
|
|
// if (robot_name && max_speed && safety_threshold) {
|
|
// LOG_INFO("Robot %s configured with:\n", robot_name);
|
|
// LOG_INFO("- Max speed: %s mm/s\n", max_speed);
|
|
// LOG_INFO("- Collision threshold: %s N\n", safety_threshold);
|
|
// }
|
|
|
|
// // Update configuration
|
|
// vtx_client_set(vtxdb, "robot_config", "temp_config.motion.max_speed", "120");
|
|
// char* updated_speed = vtx_client_get(vtxdb, "robot_config", "temp_config.motion.max_speed");
|
|
// if (updated_speed) {
|
|
// LOG_INFO("Updated max speed: %s mm/s\n", updated_speed);
|
|
// }
|
|
|
|
// // Clean up
|
|
// free(robot_name);
|
|
// free(max_speed);
|
|
// free(safety_threshold);
|
|
// free(updated_speed);
|
|
|
|
// // Delete temporary config
|
|
// vtx_client_delete(vtxdb, "robot_config", "temp_config");
|
|
// LOG_INFO("Cleaned up temporary configurations\n");
|
|
|
|
// vtx_client_close(vtxdb);
|
|
// return 0;
|
|
// }
|