feat: 基于Python ROS2的控制程序

src/peer_udp_client.c

@@ -1,6 +1,8 @@
 #include "peer_udp_client.h"
 
+#include <poll.h>
 #include <pthread.h>
+#include <string.h>
 
 struct udp_client {
     char id[OMNI_MAX_PEER_ID];
@@ -17,6 +19,19 @@ static int client_next_message_id(udp_client_t *client, uint64_t *out_id) {
     return 0;
 }
 
+static void udp_client_fill_recv_meta(udp_client_recv_meta_t *meta, const message_t *msg) {
+    if (meta == NULL || msg == NULL) {
+        return;
+    }
+    memset(meta, 0, sizeof(*meta));
+    meta->type = msg->type;
+    meta->id = msg->id;
+    meta->body_len = msg->body_len;
+    snprintf(meta->from, sizeof(meta->from), "%s", msg->from);
+    snprintf(meta->to, sizeof(meta->to), "%s", msg->to);
+    snprintf(meta->file_name, sizeof(meta->file_name), "%s", msg->file_name);
+}
+
 static int client_persist_message_to_disk(const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len) {
     char path[512];
     if (omni_ensure_dir(inbox_dir) != 0) {
@@ -70,7 +85,7 @@ static int client_persist_message_to_disk(const message_t *msg, const char *inbo
     return 0;
 }
 
-udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, const char *bind_ip, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
+udp_client_t *udp_client_dial_with_options(const char *server_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
     udp_client_t *client;
     message_t register_msg;
     client = (udp_client_t *) calloc(1, sizeof(*client));
@@ -80,7 +95,7 @@ udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, cons
     snprintf(client->id, sizeof(client->id), "%s", peer_id);
     pthread_mutex_init(&client->id_mu, NULL);
     client->logger = logger;
-    client->conn = udp_conn_dial(server_addr, bind_ip, NULL, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger);
+    client->conn = udp_conn_dial(server_addr, bind_ip, bind_device, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger);
     if (client->conn == NULL) {
         udp_client_free(client);
         return NULL;
@@ -97,6 +112,10 @@ udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, cons
     return client;
 }
 
+udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, const char *bind_ip, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
+    return udp_client_dial_with_options(server_addr, peer_id, bind_ip, NULL, logger, debug_logger, enable_timestamping);
+}
+
 const char *udp_client_id(const udp_client_t *client) {
     return client == NULL ? "" : client->id;
 }
@@ -124,6 +143,38 @@ int udp_client_send_text(udp_client_t *client, const char *to, const char *text)
     return 0;
 }
 
+int udp_client_send_binary(udp_client_t *client, const char *to, const void *data, size_t data_len) {
+    message_t msg;
+    uint64_t id;
+
+    if (client == NULL || to == NULL || (data == NULL && data_len > 0)) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    protocol_message_init(&msg);
+    client_next_message_id(client, &id);
+    msg.type = MSG_TYPE_BINARY;
+    msg.id = id;
+    snprintf(msg.from, sizeof(msg.from), "%s", client->id);
+    snprintf(msg.to, sizeof(msg.to), "%s", to);
+    if (data_len > 0) {
+        msg.body = (uint8_t *) malloc(data_len);
+        if (msg.body == NULL) {
+            return -1;
+        }
+        memcpy(msg.body, data, data_len);
+    }
+    msg.body_len = data_len;
+    latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg);
+    if (udp_conn_send(client->conn, &msg) != 0) {
+        protocol_message_clear(&msg);
+        return -1;
+    }
+    protocol_message_clear(&msg);
+    return 0;
+}
+
 int udp_client_send_file_path(udp_client_t *client, const char *to, const char *path) {
     message_t msg;
     uint64_t id;
@@ -151,7 +202,34 @@ int udp_client_send_file_path(udp_client_t *client, const char *to, const char *
     return 0;
 }
 
-int udp_client_receive(udp_client_t *client, message_t *out_msg) {
+int udp_client_receive_timed(udp_client_t *client, message_t *out_msg, int timeout_ms) {
+    if (client == NULL || out_msg == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (timeout_ms >= 0) {
+        struct pollfd pfd;
+        int rc;
+
+        memset(&pfd, 0, sizeof(pfd));
+        pfd.fd = udp_conn_fd(client->conn);
+        pfd.events = POLLIN | POLLERR | POLLHUP;
+        do {
+            rc = poll(&pfd, 1, timeout_ms);
+        } while (rc < 0 && errno == EINTR);
+        if (rc == 0) {
+            return 1;
+        }
+        if (rc < 0) {
+            return -1;
+        }
+        if ((pfd.revents & (POLLERR | POLLHUP | POLLNVAL)) != 0 && (pfd.revents & POLLIN) == 0) {
+            errno = ECONNRESET;
+            return -1;
+        }
+    }
+
     if (udp_conn_receive(client->conn, out_msg, NULL, NULL) != 0) {
         return -1;
     }
@@ -159,6 +237,39 @@ int udp_client_receive(udp_client_t *client, message_t *out_msg) {
     return 0;
 }
 
+int udp_client_receive(udp_client_t *client, message_t *out_msg) {
+    return udp_client_receive_timed(client, out_msg, -1);
+}
+
+int udp_client_receive_into(udp_client_t *client, void *buffer, size_t buffer_len, udp_client_recv_meta_t *out_meta, int timeout_ms) {
+    message_t msg;
+    int rc;
+
+    if (client == NULL || (buffer == NULL && buffer_len > 0) || out_meta == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    protocol_message_init(&msg);
+    rc = udp_client_receive_timed(client, &msg, timeout_ms);
+    if (rc != 0) {
+        return rc;
+    }
+
+    udp_client_fill_recv_meta(out_meta, &msg);
+    if (msg.body_len > buffer_len) {
+        protocol_message_clear(&msg);
+        errno = EMSGSIZE;
+        return 2;
+    }
+
+    if (msg.body_len > 0) {
+        memcpy(buffer, msg.body, msg.body_len);
+    }
+    protocol_message_clear(&msg);
+    return 0;
+}
+
 int udp_client_persist_message(udp_client_t *client, const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len) {
     if (!latencylog_is_business_message(msg)) {
         errno = EINVAL;

src/transport_kcp.c

@@ -1799,6 +1799,8 @@ int kcp_conn_close(kcp_conn_t *conn) {
         pthread_mutex_lock(&conn->kcp_mu);
         atomic_store(&conn->closed, 1);
         if (conn->owns_socket && !conn->socket_closed) {
+            /* Wake the blocking recv thread before closing the shared UDP socket. */
+            (void) shutdown(conn->fd, SHUT_RDWR);
             close(conn->fd);
             conn->socket_closed = 1;
         }

src/transport_udp.c

@@ -3,6 +3,7 @@
 #include <arpa/inet.h>
 #include <netdb.h>
 #include <pthread.h>
+#include <sys/socket.h>
 #include <unistd.h>
 
 typedef struct udp_pending_tx {
@@ -413,6 +414,13 @@ int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_stora
     }
     n = recvmsg(conn->fd, &msg, 0);
     if (n < 0) {
+        if (conn->closed) {
+            errno = ECANCELED;
+        }
+        return -1;
+    }
+    if (n == 0 && conn->closed) {
+        errno = ECANCELED;
         return -1;
     }
     if (conn->timestamping_enabled) {
@@ -454,6 +462,8 @@ int udp_conn_close(udp_conn_t *conn) {
     }
     if (!conn->closed) {
         conn->closed = 1;
+        /* Wake blocking recvmsg()/poll users before tearing down the socket. */
+        (void) shutdown(conn->fd, SHUT_RDWR);
         close(conn->fd);
         if (conn->errqueue_thread_started) {
             pthread_join(conn->errqueue_thread, NULL);