limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: C控制程序对接KCP

Browse Source
This commit is contained in:
Mock
2026-04-03 12:04:39 +08:00
parent 628583f79d
commit 8a1baa64c0
13 changed files with 1720 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
ros-control-c/common/protocol.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef PROTOCOL_H
#define PROTOCOL_H
#include <stdint.h>
#define DEFAULT_PORT 9870
#define DEFAULT_IP "127.0.0.1"
#define SEND_RATE_HZ 20
#define SEND_INTERVAL_US (1000000 / SEND_RATE_HZ)
#pragma pack(push, 1)
typedef struct {
float lx, ly, lz; /* linear velocity (m/s) */
float ax, ay, az; /* angular velocity (rad/s) */
} twist_cmd_t;
#pragma pack(pop)
#define TWIST_CMD_SIZE sizeof(twist_cmd_t) /* 24 bytes */
static inline void twist_cmd_zero(twist_cmd_t *cmd)
{
cmd->lx = cmd->ly = cmd->lz = 0.0f;
cmd->ax = cmd->ay = cmd->az = 0.0f;
}
#endif /* PROTOCOL_H */

300
ros-control-c/common/teleop_transport.c Normal file
View File

@@ -0,0 +1,300 @@
#include "teleop_transport.h"
#include <arpa/inet.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>
static void teleop_transport_clear(teleop_transport_t *transport)
{
if (transport == NULL) {
return;
}
memset(transport, 0, sizeof(*transport));
transport->mode = TELEOP_TRANSPORT_MODE_UDP;
transport->udp_fd = -1;
}
int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode)
{
if (raw == NULL || out_mode == NULL) {
errno = EINVAL;
return -1;
}
if (strcmp(raw, "udp") == 0) {
*out_mode = TELEOP_TRANSPORT_MODE_UDP;
return 0;
}
if (strcmp(raw, "kcp") == 0) {
*out_mode = TELEOP_TRANSPORT_MODE_KCP;
return 0;
}
errno = EINVAL;
return -1;
}
const char *teleop_transport_mode_name(teleop_transport_mode_t mode)
{
return mode == TELEOP_TRANSPORT_MODE_KCP ? "kcp" : "udp";
}
static void teleop_transport_log_incoming(const message_t *msg)
{
if (msg == NULL) {
return;
}
switch (msg->type) {
case MSG_TYPE_ERROR:
fprintf(stderr,
"teleop transport: server error from %s to %s: %.*s\n",
msg->from,
msg->to,
(int)msg->body_len,
msg->body == NULL ? "" : (const char *)msg->body);
break;
case MSG_TYPE_TEXT:
fprintf(stderr,
"teleop transport: dropped unexpected text from %s to %s: %.*s\n",
msg->from,
msg->to,
(int)msg->body_len,
msg->body == NULL ? "" : (const char *)msg->body);
break;
case MSG_TYPE_BINARY:
fprintf(stderr,
"teleop transport: dropped unexpected binary payload from %s to %s (%lu bytes)\n",
msg->from,
msg->to,
(unsigned long)msg->body_len);
break;
case MSG_TYPE_FILE:
fprintf(stderr,
"teleop transport: dropped unexpected file from %s to %s: %s (%lu bytes)\n",
msg->from,
msg->to,
msg->file_name,
(unsigned long)msg->body_len);
break;
case MSG_TYPE_REGISTER:
fprintf(stderr,
"teleop transport: dropped unexpected register message from %s to %s\n",
msg->from,
msg->to);
break;
default:
fprintf(stderr,
"teleop transport: dropped unexpected message type %s from %s\n",
protocol_message_type_name(msg->type),
msg->from);
break;
}
}
static void *teleop_transport_kcp_recv_thread_main(void *arg)
{
teleop_transport_t *transport = (teleop_transport_t *)arg;
for (;;) {
message_t msg;
int rc;
if (transport->stop_requested) {
return NULL;
}
protocol_message_init(&msg);
rc = kcp_client_receive_timed(transport->kcp_client, &msg, 100);
if (rc == 1) {
protocol_message_clear(&msg);
continue;
}
if (rc != 0) {
protocol_message_clear(&msg);
if (!transport->stop_requested) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: KCP receive loop stopped: %s (errno=%d)\n",
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
}
return NULL;
}
teleop_transport_log_incoming(&msg);
protocol_message_clear(&msg);
}
}
static int teleop_transport_open_udp(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
int sockfd;
if (transport == NULL || config == NULL || config->udp_ip == NULL) {
errno = EINVAL;
return -1;
}
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
perror("socket");
return -1;
}
memset(&transport->udp_dest, 0, sizeof(transport->udp_dest));
transport->udp_dest.sin_family = AF_INET;
transport->udp_dest.sin_port = htons(config->udp_port);
if (inet_pton(AF_INET, config->udp_ip, &transport->udp_dest.sin_addr) <= 0) {
fprintf(stderr, "Invalid IP: %s\n", config->udp_ip);
close(sockfd);
errno = EINVAL;
return -1;
}
transport->udp_fd = sockfd;
return 0;
}
static int teleop_transport_open_kcp(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
kcp_conn_options_t options;
const char *relay_via;
if (transport == NULL || config == NULL ||
config->server_addr == NULL || config->peer_id == NULL || config->target_peer == NULL) {
errno = EINVAL;
return -1;
}
kcp_conn_options_set_control_defaults(&options);
relay_via = (config->relay_via != NULL && config->relay_via[0] != '\0') ? config->relay_via : NULL;
transport->kcp_client = kcp_client_dial_with_options(
config->server_addr,
relay_via,
config->peer_id,
"",
"",
&options,
NULL,
NULL,
NULL,
KCP_DEFAULT_STATS_INTERVAL_MS
);
if (transport->kcp_client == NULL) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: failed to open KCP session as %s via %s%s%s: %s (errno=%d)\n",
config->peer_id,
config->server_addr,
relay_via != NULL ? ", relay=" : "",
relay_via != NULL ? relay_via : "",
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
errno = saved_errno;
return -1;
}
{
int thread_rc = pthread_create(&transport->recv_thread, NULL, teleop_transport_kcp_recv_thread_main, transport);
if (thread_rc != 0) {
fprintf(stderr,
"teleop transport: failed to start KCP receive thread: %s (errno=%d)\n",
strerror(thread_rc),
thread_rc);
kcp_client_close(transport->kcp_client);
kcp_client_free(transport->kcp_client);
transport->kcp_client = NULL;
errno = thread_rc;
return -1;
}
}
transport->recv_thread_started = 1;
return 0;
}
int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config)
{
if (transport == NULL || config == NULL) {
errno = EINVAL;
return -1;
}
teleop_transport_clear(transport);
transport->mode = config->mode;
snprintf(transport->server_addr, sizeof(transport->server_addr), "%s",
config->server_addr == NULL ? "" : config->server_addr);
snprintf(transport->relay_via, sizeof(transport->relay_via), "%s",
config->relay_via == NULL ? "" : config->relay_via);
snprintf(transport->peer_id, sizeof(transport->peer_id), "%s",
config->peer_id == NULL ? "" : config->peer_id);
snprintf(transport->target_peer, sizeof(transport->target_peer), "%s",
config->target_peer == NULL ? "" : config->target_peer);
if (config->mode == TELEOP_TRANSPORT_MODE_KCP) {
return teleop_transport_open_kcp(transport, config);
}
return teleop_transport_open_udp(transport, config);
}
int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd)
{
if (transport == NULL || cmd == NULL) {
errno = EINVAL;
return -1;
}
if (transport->mode == TELEOP_TRANSPORT_MODE_KCP) {
if (kcp_client_send_binary(transport->kcp_client, transport->target_peer, cmd, TWIST_CMD_SIZE) != 0) {
int saved_errno = errno;
fprintf(stderr,
"teleop transport: failed to send KCP payload to %s: %s (errno=%d)\n",
transport->target_peer,
saved_errno != 0 ? strerror(saved_errno) : "unknown error",
saved_errno);
errno = saved_errno;
return -1;
}
return 0;
}
{
ssize_t sent = sendto(transport->udp_fd, cmd, TWIST_CMD_SIZE, 0,
(const struct sockaddr *)&transport->udp_dest, sizeof(transport->udp_dest));
if (sent < 0) {
perror("sendto");
return -1;
}
if ((size_t)sent != TWIST_CMD_SIZE) {
fprintf(stderr, "sendto: short send (%zd/%zu)\n", sent, (size_t)TWIST_CMD_SIZE);
errno = EIO;
return -1;
}
}
return 0;
}
void teleop_transport_close(teleop_transport_t *transport)
{
if (transport == NULL) {
return;
}
transport->stop_requested = 1;
if (transport->kcp_client != NULL) {
kcp_client_close(transport->kcp_client);
}
if (transport->recv_thread_started) {
pthread_join(transport->recv_thread, NULL);
transport->recv_thread_started = 0;
}
if (transport->kcp_client != NULL) {
kcp_client_free(transport->kcp_client);
transport->kcp_client = NULL;
}
if (transport->udp_fd >= 0) {
close(transport->udp_fd);
transport->udp_fd = -1;
}
}

59
ros-control-c/common/teleop_transport.h Normal file
View File

@@ -0,0 +1,59 @@
#ifndef TELEOP_TRANSPORT_H
#define TELEOP_TRANSPORT_H
#include <netinet/in.h>
#include <pthread.h>
#include "protocol.h"
#include "peer_kcp_client.h"
#ifdef __cplusplus
extern "C" {
#endif
#define DEFAULT_KCP_SERVER_ADDR "127.0.0.1:9002"
#define DEFAULT_KCP_KEYBOARD_PEER_ID "ros-keyboard-ctrl"
#define DEFAULT_KCP_GAMEPAD_PEER_ID "ros-gamepad-ctrl"
#define DEFAULT_KCP_TARGET_PEER_ID "ros-bridge-ctrl"
typedef enum teleop_transport_mode {
TELEOP_TRANSPORT_MODE_UDP = 0,
TELEOP_TRANSPORT_MODE_KCP = 1
} teleop_transport_mode_t;
typedef struct teleop_transport_config {
teleop_transport_mode_t mode;
const char *udp_ip;
int udp_port;
const char *server_addr;
const char *relay_via;
const char *peer_id;
const char *target_peer;
} teleop_transport_config_t;
typedef struct teleop_transport {
teleop_transport_mode_t mode;
int udp_fd;
struct sockaddr_in udp_dest;
kcp_client_t *kcp_client;
pthread_t recv_thread;
int recv_thread_started;
volatile int stop_requested;
char server_addr[OMNI_MAX_ADDR_TEXT];
char relay_via[OMNI_MAX_ADDR_TEXT];
char peer_id[OMNI_MAX_PEER_ID];
char target_peer[OMNI_MAX_PEER_ID];
} teleop_transport_t;
int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode);
const char *teleop_transport_mode_name(teleop_transport_mode_t mode);
int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config);
int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd);
void teleop_transport_close(teleop_transport_t *transport);
#ifdef __cplusplus
}
#endif
#endif /* TELEOP_TRANSPORT_H */