Add udp2lcm server && datastructure to wheel ctrl

7 files changed, 602 insertions, 431 deletions

diff --git a/path/path_ctrl.lcm b/path/path_ctrl.lcm
new file mode 100644
index 0000000..502e342
--- /dev/null
+++ b/path/path_ctrl.lcm
@@ -0,0 +1,5 @@
+struct path_ctrl_t
+{
+    int8_t cmd;
+    int8_t speed;
+}
diff --git a/path/path_ctrl_t.c b/path/path_ctrl_t.c
new file mode 100644
index 0000000..2298a56
--- /dev/null
+++ b/path/path_ctrl_t.c
@@ -0,0 +1,241 @@
+// THIS IS AN AUTOMATICALLY GENERATED FILE.  DO NOT MODIFY
+// BY HAND!!
+//
+// Generated by lcm-gen
+
+#include <string.h>
+#include "path_ctrl_t.h"
+
+static int __path_ctrl_t_hash_computed;
+static uint64_t __path_ctrl_t_hash;
+
+uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p)
+{
+    const __lcm_hash_ptr *fp;
+    for (fp = p; fp != NULL; fp = fp->parent)
+        if (fp->v == __path_ctrl_t_get_hash)
+            return 0;
+
+    __lcm_hash_ptr cp;
+    cp.parent =  p;
+    cp.v = (void*)__path_ctrl_t_get_hash;
+    (void) cp;
+
+    uint64_t hash = (uint64_t)0x5cf946463c463b9eLL
+         + __int8_t_hash_recursive(&cp)
+         + __int8_t_hash_recursive(&cp)
+        ;
+
+    return (hash<<1) + ((hash>>63)&1);
+}
+
+int64_t __path_ctrl_t_get_hash(void)
+{
+    if (!__path_ctrl_t_hash_computed) {
+        __path_ctrl_t_hash = (int64_t)__path_ctrl_t_hash_recursive(NULL);
+        __path_ctrl_t_hash_computed = 1;
+    }
+
+    return __path_ctrl_t_hash;
+}
+
+int __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements)
+{
+    int pos = 0, element;
+    int thislen;
+
+    for (element = 0; element < elements; element++) {
+
+        thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1);
+        if (thislen < 0) return thislen; else pos += thislen;
+
+        thislen = __int8_t_encode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1);
+        if (thislen < 0) return thislen; else pos += thislen;
+
+    }
+    return pos;
+}
+
+int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p)
+{
+    int pos = 0, thislen;
+    int64_t hash = __path_ctrl_t_get_hash();
+
+    thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);
+    if (thislen < 0) return thislen; else pos += thislen;
+
+    thislen = __path_ctrl_t_encode_array(buf, offset + pos, maxlen - pos, p, 1);
+    if (thislen < 0) return thislen; else pos += thislen;
+
+    return pos;
+}
+
+int __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements)
+{
+    int size = 0, element;
+    for (element = 0; element < elements; element++) {
+
+        size += __int8_t_encoded_array_size(&(p[element].cmd), 1);
+
+        size += __int8_t_encoded_array_size(&(p[element].speed), 1);
+
+    }
+    return size;
+}
+
+int path_ctrl_t_encoded_size(const path_ctrl_t *p)
+{
+    return 8 + __path_ctrl_t_encoded_array_size(p, 1);
+}
+
+int __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements)
+{
+    int pos = 0, thislen, element;
+
+    for (element = 0; element < elements; element++) {
+
+        thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].cmd), 1);
+        if (thislen < 0) return thislen; else pos += thislen;
+
+        thislen = __int8_t_decode_array(buf, offset + pos, maxlen - pos, &(p[element].speed), 1);
+        if (thislen < 0) return thislen; else pos += thislen;
+
+    }
+    return pos;
+}
+
+int __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements)
+{
+    int element;
+    for (element = 0; element < elements; element++) {
+
+        __int8_t_decode_array_cleanup(&(p[element].cmd), 1);
+
+        __int8_t_decode_array_cleanup(&(p[element].speed), 1);
+
+    }
+    return 0;
+}
+
+int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *p)
+{
+    int pos = 0, thislen;
+    int64_t hash = __path_ctrl_t_get_hash();
+
+    int64_t this_hash;
+    thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);
+    if (thislen < 0) return thislen; else pos += thislen;
+    if (this_hash != hash) return -1;
+
+    thislen = __path_ctrl_t_decode_array(buf, offset + pos, maxlen - pos, p, 1);
+    if (thislen < 0) return thislen; else pos += thislen;
+
+    return pos;
+}
+
+int path_ctrl_t_decode_cleanup(path_ctrl_t *p)
+{
+    return __path_ctrl_t_decode_array_cleanup(p, 1);
+}
+
+int __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements)
+{
+    int element;
+    for (element = 0; element < elements; element++) {
+
+        __int8_t_clone_array(&(p[element].cmd), &(q[element].cmd), 1);
+
+        __int8_t_clone_array(&(p[element].speed), &(q[element].speed), 1);
+
+    }
+    return 0;
+}
+
+path_ctrl_t *path_ctrl_t_copy(const path_ctrl_t *p)
+{
+    path_ctrl_t *q = (path_ctrl_t*) malloc(sizeof(path_ctrl_t));
+    __path_ctrl_t_clone_array(p, q, 1);
+    return q;
+}
+
+void path_ctrl_t_destroy(path_ctrl_t *p)
+{
+    __path_ctrl_t_decode_array_cleanup(p, 1);
+    free(p);
+}
+
+int path_ctrl_t_publish(lcm_t *lc, const char *channel, const path_ctrl_t *p)
+{
+      int max_data_size = path_ctrl_t_encoded_size (p);
+      uint8_t *buf = (uint8_t*) malloc (max_data_size);
+      if (!buf) return -1;
+      int data_size = path_ctrl_t_encode (buf, 0, max_data_size, p);
+      if (data_size < 0) {
+          free (buf);
+          return data_size;
+      }
+      int status = lcm_publish (lc, channel, buf, data_size);
+      free (buf);
+      return status;
+}
+
+struct _path_ctrl_t_subscription_t {
+    path_ctrl_t_handler_t user_handler;
+    void *userdata;
+    lcm_subscription_t *lc_h;
+};
+static
+void path_ctrl_t_handler_stub (const lcm_recv_buf_t *rbuf,
+                            const char *channel, void *userdata)
+{
+    int status;
+    path_ctrl_t p;
+    memset(&p, 0, sizeof(path_ctrl_t));
+    status = path_ctrl_t_decode (rbuf->data, 0, rbuf->data_size, &p);
+    if (status < 0) {
+        fprintf (stderr, "error %d decoding path_ctrl_t!!!\n", status);
+        return;
+    }
+
+    path_ctrl_t_subscription_t *h = (path_ctrl_t_subscription_t*) userdata;
+    h->user_handler (rbuf, channel, &p, h->userdata);
+
+    path_ctrl_t_decode_cleanup (&p);
+}
+
+path_ctrl_t_subscription_t* path_ctrl_t_subscribe (lcm_t *lcm,
+                    const char *channel,
+                    path_ctrl_t_handler_t f, void *userdata)
+{
+    path_ctrl_t_subscription_t *n = (path_ctrl_t_subscription_t*)
+                       malloc(sizeof(path_ctrl_t_subscription_t));
+    n->user_handler = f;
+    n->userdata = userdata;
+    n->lc_h = lcm_subscribe (lcm, channel,
+                                 path_ctrl_t_handler_stub, n);
+    if (n->lc_h == NULL) {
+        fprintf (stderr,"couldn't reg path_ctrl_t LCM handler!\n");
+        free (n);
+        return NULL;
+    }
+    return n;
+}
+
+int path_ctrl_t_subscription_set_queue_capacity (path_ctrl_t_subscription_t* subs,
+                              int num_messages)
+{
+    return lcm_subscription_set_queue_capacity (subs->lc_h, num_messages);
+}
+
+int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid)
+{
+    int status = lcm_unsubscribe (lcm, hid->lc_h);
+    if (0 != status) {
+        fprintf(stderr,
+           "couldn't unsubscribe path_ctrl_t_handler %p!\n", hid);
+        return -1;
+    }
+    free (hid);
+    return 0;
+}
+
diff --git a/path/path_ctrl_t.h b/path/path_ctrl_t.h
new file mode 100644
index 0000000..3e629eb
--- /dev/null
+++ b/path/path_ctrl_t.h
@@ -0,0 +1,143 @@
+// THIS IS AN AUTOMATICALLY GENERATED FILE.  DO NOT MODIFY
+// BY HAND!!
+//
+// Generated by lcm-gen
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <lcm/lcm_coretypes.h>
+#include <lcm/lcm.h>
+
+#ifndef _path_ctrl_t_h
+#define _path_ctrl_t_h
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct _path_ctrl_t path_ctrl_t;
+struct _path_ctrl_t
+{
+    int8_t     cmd;
+    int8_t     speed;
+};
+
+/**
+ * Create a deep copy of a path_ctrl_t.
+ * When no longer needed, destroy it with path_ctrl_t_destroy()
+ */
+path_ctrl_t* path_ctrl_t_copy(const path_ctrl_t* to_copy);
+
+/**
+ * Destroy an instance of path_ctrl_t created by path_ctrl_t_copy()
+ */
+void path_ctrl_t_destroy(path_ctrl_t* to_destroy);
+
+/**
+ * Identifies a single subscription.  This is an opaque data type.
+ */
+typedef struct _path_ctrl_t_subscription_t path_ctrl_t_subscription_t;
+
+/**
+ * Prototype for a callback function invoked when a message of type
+ * path_ctrl_t is received.
+ */
+typedef void(*path_ctrl_t_handler_t)(const lcm_recv_buf_t *rbuf,
+             const char *channel, const path_ctrl_t *msg, void *userdata);
+
+/**
+ * Publish a message of type path_ctrl_t using LCM.
+ *
+ * @param lcm The LCM instance to publish with.
+ * @param channel The channel to publish on.
+ * @param msg The message to publish.
+ * @return 0 on success, <0 on error.  Success means LCM has transferred
+ * responsibility of the message data to the OS.
+ */
+int path_ctrl_t_publish(lcm_t *lcm, const char *channel, const path_ctrl_t *msg);
+
+/**
+ * Subscribe to messages of type path_ctrl_t using LCM.
+ *
+ * @param lcm The LCM instance to subscribe with.
+ * @param channel The channel to subscribe to.
+ * @param handler The callback function invoked by LCM when a message is received.
+ *                This function is invoked by LCM during calls to lcm_handle() and
+ *                lcm_handle_timeout().
+ * @param userdata An opaque pointer passed to @p handler when it is invoked.
+ * @return 0 on success, <0 if an error occured
+ */
+path_ctrl_t_subscription_t* path_ctrl_t_subscribe(lcm_t *lcm, const char *channel, path_ctrl_t_handler_t handler, void *userdata);
+
+/**
+ * Removes and destroys a subscription created by path_ctrl_t_subscribe()
+ */
+int path_ctrl_t_unsubscribe(lcm_t *lcm, path_ctrl_t_subscription_t* hid);
+
+/**
+ * Sets the queue capacity for a subscription.
+ * Some LCM providers (e.g., the default multicast provider) are implemented
+ * using a background receive thread that constantly revceives messages from
+ * the network.  As these messages are received, they are buffered on
+ * per-subscription queues until dispatched by lcm_handle().  This function
+ * how many messages are queued before dropping messages.
+ *
+ * @param subs the subscription to modify.
+ * @param num_messages The maximum number of messages to queue
+ *  on the subscription.
+ * @return 0 on success, <0 if an error occured
+ */
+int path_ctrl_t_subscription_set_queue_capacity(path_ctrl_t_subscription_t* subs,
+                              int num_messages);
+
+/**
+ * Encode a message of type path_ctrl_t into binary form.
+ *
+ * @param buf The output buffer.
+ * @param offset Encoding starts at this byte offset into @p buf.
+ * @param maxlen Maximum number of bytes to write.  This should generally
+ *               be equal to path_ctrl_t_encoded_size().
+ * @param msg The message to encode.
+ * @return The number of bytes encoded, or <0 if an error occured.
+ */
+int path_ctrl_t_encode(void *buf, int offset, int maxlen, const path_ctrl_t *p);
+
+/**
+ * Decode a message of type path_ctrl_t from binary form.
+ * When decoding messages containing strings or variable-length arrays, this
+ * function may allocate memory.  When finished with the decoded message,
+ * release allocated resources with path_ctrl_t_decode_cleanup().
+ *
+ * @param buf The buffer containing the encoded message
+ * @param offset The byte offset into @p buf where the encoded message starts.
+ * @param maxlen The maximum number of bytes to read while decoding.
+ * @param msg Output parameter where the decoded message is stored
+ * @return The number of bytes decoded, or <0 if an error occured.
+ */
+int path_ctrl_t_decode(const void *buf, int offset, int maxlen, path_ctrl_t *msg);
+
+/**
+ * Release resources allocated by path_ctrl_t_decode()
+ * @return 0
+ */
+int path_ctrl_t_decode_cleanup(path_ctrl_t *p);
+
+/**
+ * Check how many bytes are required to encode a message of type path_ctrl_t
+ */
+int path_ctrl_t_encoded_size(const path_ctrl_t *p);
+
+// LCM support functions. Users should not call these
+int64_t __path_ctrl_t_get_hash(void);
+uint64_t __path_ctrl_t_hash_recursive(const __lcm_hash_ptr *p);
+int     __path_ctrl_t_encode_array(void *buf, int offset, int maxlen, const path_ctrl_t *p, int elements);
+int     __path_ctrl_t_decode_array(const void *buf, int offset, int maxlen, path_ctrl_t *p, int elements);
+int     __path_ctrl_t_decode_array_cleanup(path_ctrl_t *p, int elements);
+int     __path_ctrl_t_encoded_array_size(const path_ctrl_t *p, int elements);
+int     __path_ctrl_t_clone_array(const path_ctrl_t *p, path_ctrl_t *q, int elements);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/serial.c b/serial.c
index 43506ec..6251675 100644
--- a/serial.c
+++ b/serial.c
@@ -1,50 +1,46 @@
-/*
- * 在OpenHarmony系统下开发串口通信的应用与在Linux系统中类似，
- * 但需要确保你的设备支持串口操作，并且有适当的驱动支持。
- *
- * 以下是一个用C++编写的示例，展示如何在OpenHarmony中打开串口，
- * 设置波特率为115200，并发送指定的16进制数。
- *   - 打开串口：使用open()函数打开指定的串口设备。
- *   - 配置串口：使用termios结构体配置串口参数如波特率、字符大小、奇偶校验等。
- *   - 发送数据：使用write()函数将数据写入串口。
- *   - 关闭串口：操作完成后，使用close()函数关闭串口。
- * 确保你的OpenHarmony设备有对应的串口设备文件/dev/ttyS1，
- * 并且你的应用有足够的权限来访问这个设备。根据你的硬件和操作系统配置，设备文件的路径可能不同。
- */
-
-#include <arpa/inet.h>
+#include "path/path_ctrl_t.h"
 #include <errno.h>
 #include <fcntl.h>
-#include <netinet/in.h>
-#include <poll.h>
-#include <pthread.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <sys/socket.h>
 #include <termios.h>
 #include <unistd.h>
 
-#define FULL_SPEED 1.0f // 全速前进的速度
-#define R 0.15f         // 轮距的一半
-
 typedef unsigned char byte;
-int fd;
-int port = 5001;
+#define MAX_BUFFER_SIZE 1024
+
+int fd;                             // 轮子的串口文件描述符
 char portname[50] = "/dev/ttyUSB0"; // 串口设备名
 char clientIP[20] = "";
-
 struct termios tty;
-pthread_t udpSend, udpRecv;
-const int MAX_BUFFER_SIZE = 1024;
-struct sockaddr_in clientAddr;
 
-int whellInit() {
+bool whellInit();
+bool wheelSend(byte a, byte a_v, byte b, byte b_v);
+void speedControl(byte status, byte speed);
+void parseCmd(const path_ctrl_t *msg, void *user);
+
+int main() {
+    if (!whellInit()) {
+        goto err;
+    }
+    lcm *path_lcm = lcm_create(NULL);
+    if (!path_lcm) {
+        fprintf(stderr, "Failed to create LCM\n");
+        goto err;
+    }
+    path_ctrl_t_subscribe(path_lcm, "PATH_CTRL", parseCmd);
+
+err:
+    close(fd);
+    return 0;
+}
+
+bool whellInit() {
     // 打开串口
     fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
     if (fd < 0) {
-        // C语言报error
         fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno));
         return false;
     }
@@ -52,7 +48,6 @@ int whellInit() {
     // 设置串口参数
     memset(&tty, 0, sizeof tty);
     if (tcgetattr(fd, &tty) != 0) {
-        // cerr << "Error from tcgetattr: " << strerror(errno) << endl;
         fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno));
         return false;
     }
@@ -77,7 +72,6 @@ int whellInit() {
     tty.c_cflag &= ~CRTSCTS;
 
     if (tcsetattr(fd, TCSANOW, &tty) != 0) {
-        // cerr << "Error from tcsetattr: " << strerror(errno) << endl;
         fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno));
         return false;
     }
@@ -108,7 +102,8 @@ bool wheelSend(byte a, byte a_v, byte b, byte b_v) {
     return true;
 }
 
-void speedControl(byte status, byte speed) {
+void parseCmd(const path_ctrl_t *msg, void *user) {
+    byte status = msg->cmd, speed = msg->speed;
     switch (status) {
     case 1:
         wheelSend(0x01, speed, 0x01, speed);
@@ -124,137 +119,4 @@ void speedControl(byte status, byte speed) {
         wheelSend(0x00, 0x00, 0x00, 0x00);
         break;
     }
-}
-
-void udpSendThread() {
-    // 创建UDP套接字
-    int sockfd = -1;
-    struct sockaddr_in serverAddr;
-    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    serverAddr.sin_family = AF_INET;   // 设置地址族为IPv4
-    serverAddr.sin_port = htons(port); // 设置端口号
-    if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
-        fprintf(stderr, "Error: Failed to convert IP address\n");
-        close(sockfd);
-        return;
-    }
-    while (true) {
-        // 每秒向socket发送一次0x30
-        byte a = 0x39;
-        // 发送消息
-        sendto(sockfd, (const char *)&a, 1, 0, (struct sockaddr *)&serverAddr,
-               sizeof(serverAddr));
-        printf("Send: Data %02X udpSend successfully!\n", a);
-        sleep(1);
-    }
-}
-
-void udpRecvThread() {
-    int socketfd = -1;
-    char buffer[MAX_BUFFER_SIZE];
-    struct sockaddr_in serverAddr;
-    socklen_t addrLen = sizeof(serverAddr);
-    int bytesReceived;
-
-    // fd_set readfds;
-    // struct timeval tv;
-    int retval;
-    struct pollfd fds;
-
-    if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    memset(buffer, 0, sizeof(buffer));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(port);
-    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
-
-    if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-        -1) {
-        // cerr << "Error: Bind failed" << endl;
-        fprintf(stderr, "Error: Bind failed\n");
-        close(socketfd);
-        return;
-    }
-
-    bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
-                             (struct sockaddr *)&clientAddr, &addrLen);
-    if (bytesReceived == -1) {
-        fprintf(stderr, "Error: Failed to receive data\n");
-        close(socketfd);
-        return;
-    }
-    printf("Received start message from %s: %s\n",
-           inet_ntoa(clientAddr.sin_addr), buffer);
-    strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
-
-    if (pthread_create(&udpSend, NULL, udpSendThread, NULL) != 0) {
-        fprintf(stderr, "Error creating udpSend thread\n");
-        close(socketfd);
-        return;
-    }
-
-    fds.fd = socketfd;
-    fds.events = POLLIN;
-
-    while (true) {
-        // /* Watch sock to see when it has input. */
-        // FD_ZERO(&readfds);
-        // FD_SET(socketfd, &readfds);
-        // /* Wait up to three seconds. */
-        // tv.tv_sec = 3;
-        // tv.tv_usec = 0;
-
-        // retval = select(socketfd + 1, &readfds, NULL, NULL, &tv);
-        retval = poll(&fds, 1, 3000);
-        if (retval == -1) {
-            fprintf(stderr, "Error: select failed\n");
-            break;
-        } else if (retval == 0) {
-            printf("No data within three seconds.\n");
-            speedControl(0, 0);
-            // break;
-        } else {
-            bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
-                                     (struct sockaddr *)&clientAddr, &addrLen);
-            if (bytesReceived == -1) {
-                fprintf(stderr, "Error: Failed to receive data\n");
-                continue;
-            }
-            printf("Received Message from %s: %2x %2x\n",
-                   inet_ntoa(clientAddr.sin_addr), buffer[0], buffer[1]);
-            speedControl(buffer[0], buffer[1]);
-        }
-    }
-}
-
-int main() {
-    // 关闭1号和2号文件描述符
-    // close(1);
-    // close(2);
-    // // 打开serial.log文件，并将其复制到1号和2号文件描述符
-    // open("serial.log", O_RDWR | O_CREAT, 0666);
-    // dup(1);
-    // dup(2);
-
-    if (!whellInit()) {
-        goto err;
-    }
-
-    if (pthread_create(&udpRecv, NULL, udpRecvThread, NULL) != 0) {
-        fprintf(stderr, "Error creating udpRecv thread\n");
-        goto err;
-    }
-    pthread_join(udpRecv, NULL);
-
-err:
-    close(fd);
-    return 0;
-}
+}
\ No newline at end of file
diff --git a/server.cpp b/server.cpp
deleted file mode 100644
index 5e41eb8..0000000
--- a/server.cpp
+++ /dev/null
@@ -1,167 +0,0 @@
-#include <arpa/inet.h>
-#include <chrono>
-#include <cmath>
-#include <condition_variable>
-#include <cstring>
-#include <iostream>
-#include <mutex>
-#include <netinet/in.h>
-#include <sstream>
-#include <string.h>
-#include <string>
-#include <sys/socket.h>
-#include <thread>
-#include <unistd.h>
-
-using namespace std;
-
-enum robotState { // 机器人状态枚举
-    beforeStart,
-    pending,
-    afterEnd, // 建好了图，接下来可选择重新建图或选终点开始路径
-    startRoute,
-};
-
-enum runState { stop, go, left, right };
-
-struct ReceiveDataStruct {
-    int robotId;
-    robotState state;
-    int x;
-    int y;
-    int r;
-};
-
-const int PORT = 5001;
-const char *SERVER_IP = "0.0.0.0";
-const int MAX_BUFFER_SIZE = 1024;
-
-struct ReceiveData {
-    int values[4];
-};
-
-bool newDataAvailable = false;
-string CLIENT_IP = "127.0.0.1";
-struct sockaddr_in clientAddr;
-ReceiveData receivedData = {{0, 1, 2, 3}};
-
-// UDP发送线程函数
-void udpSenderThread() {
-    char values[100] = {0};
-    struct sockaddr_in serverAddr;
-    int sockfd = -1;
-
-    // 创建UDP套接字
-    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(PORT);
-    if (inet_aton(CLIENT_IP.c_str(), &serverAddr.sin_addr) == 0) {
-        fprintf(stderr, "Error: Failed to convert IP address\n");
-        close(sockfd);
-        return;
-    }
-
-    while (true) {
-        this_thread::sleep_for(chrono::seconds(1)); // 每1秒发送一次
-
-        // 更新发送的消息
-        memset(values, 0, 100);
-        for (int i = 0; i < 4; ++i) {
-            // values[i] = receivedData.values[i] + 1;
-            sprintf(values + strlen(values), "%d ", receivedData.values[i] + 1);
-        }
-        sprintf(values + strlen(values), "\n");
-        printf("Sending message to %s: %s\n", CLIENT_IP.c_str(), values);
-
-        // 发送消息
-        sendto(sockfd, (const char *)values, sizeof(int) * 4, 0,
-               (struct sockaddr *)&serverAddr, sizeof(serverAddr));
-    }
-
-    close(sockfd);
-}
-
-// 主线程接收UDP消息并打印
-void receiveAndPrintMessages() {
-    int sock;
-    struct sockaddr_in serverAddr;
-    socklen_t addrLen = sizeof(clientAddr);
-    char buffer[MAX_BUFFER_SIZE];
-    ReceiveData receivedData;
-    int pos, bytesReceived;
-
-    // 创建UDP套接字
-    if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    memset(buffer, 0, sizeof(buffer));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(PORT);
-    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
-
-    // 绑定到端口
-    if (::bind(sock, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-        -1) {
-        cerr << "Error: Bind failed" << endl;
-        close(sock);
-        return;
-    }
-
-    bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0,
-                             (struct sockaddr *)&clientAddr, &addrLen);
-    if (bytesReceived == -1) {
-        fprintf(stderr, "Error: Failed to receive data\n");
-        return;
-    }
-    printf("Received Message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
-           buffer);
-    CLIENT_IP = inet_ntoa(clientAddr.sin_addr);
-    thread senderThread2(udpSenderThread);
-    while (true) {
-        // 接收消息
-        bytesReceived = recvfrom(sock, buffer, MAX_BUFFER_SIZE, 0,
-                                 (struct sockaddr *)&clientAddr, &addrLen);
-        if (bytesReceived == -1) {
-            fprintf(stderr, "Error: Failed to receive data\n");
-            continue;
-        }
-
-        printf("Receive %d bytes from %s: \"%s\"\n", bytesReceived,
-               CLIENT_IP.c_str(), buffer);
-        pos = 0;
-        for (int i = 0; i < 4; ++i) {
-            receivedData.values[i] = atoi(strtok(buffer + pos, " "));
-            // pos += log10(receivedData.values[i]) + 2;
-            // pos需要递增，否则会一直解析第一个数，要求能处理数据为0或负数的情况
-            pos += strlen(strtok(buffer + pos, " ")) + 1;
-        }
-        newDataAvailable = true;
-
-        // 打印消息
-        // CLIENT_IP = inet_ntoa(clientAddr.sin_addr);
-        // cout << "Received Message from " << CLIENT_IP << ": ";
-        // printf("Received Message from %s: ", CLIENT_IP.c_str());
-        // for (int i = 0; i < 4; ++i) {
-        //     printf("%d ", receivedData.values[i]);
-        // }
-        printf("\n");
-    }
-
-    close(sock);
-    senderThread2.join();
-}
-
-int main() {
-    thread receiverThread(receiveAndPrintMessages);
-    receiverThread.join();
-
-    return 0;
-}
diff --git a/testClient.c b/testClient.c
deleted file mode 100644
index bbd2529..0000000
--- a/testClient.c
+++ /dev/null
@@ -1,97 +0,0 @@
-// 编写一段C程序，使用pthread创建两个线程
-// 发送线程将数据发送到127.0.0.1的5001端口，每0.5s一次
-// 接收线程接收127.0.0.1的5001端口的数据并打印
-
-#include <arpa/inet.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <netinet/in.h>
-#include <pthread.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/socket.h>
-#include <termios.h>
-#include <unistd.h>
-
-pthread_t udpSend, udpRecv;
-pthread_mutex_t mutex;
-int port = 5001;
-char clientIP[20] = "127.0.0.1";
-
-void udpSenderThread() {
-    struct sockaddr_in serverAddr;
-    int sockfd = -1;
-
-    // 创建UDP套接字
-    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(port);
-    serverAddr.sin_addr.s_addr = inet_addr(clientIP);
-
-    while (1) {
-        char values[100] = {0};
-        // strcpy(values, "Hello, world!");
-        pthread_mutex_lock(&mutex);
-        printf("Input the data you want to send: ");
-        scanf("%s", values);
-        pthread_mutex_unlock(&mutex);
-        sendto(sockfd, values, strlen(values), 0,
-               (struct sockaddr *)&serverAddr, sizeof(serverAddr));
-        // printf("Send: %s\n", values);
-        sleep(1);
-    }
-}
-
-void udpReceiverThread() {
-    int sockfd = -1;
-    struct sockaddr_in serverAddr;
-    struct sockaddr_in clientAddr;
-    socklen_t clientAddrLen = sizeof(clientAddr);
-    char buffer[1024] = {0};
-
-    // 创建UDP套接字
-    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(port - 1);
-    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
-
-    if (bind(sockfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-        -1) {
-        fprintf(stderr, "Error: Failed to bind socket\n");
-        close(sockfd);
-        return;
-    }
-
-    while (1) {
-        pthread_mutex_lock(&mutex);
-        int len = recvfrom(sockfd, buffer, 1024, 0,
-                           (struct sockaddr *)&clientAddr, &clientAddrLen);
-        pthread_mutex_unlock(&mutex);
-        if (len == -1) {
-            fprintf(stderr, "Error: Failed to receive data\n");
-            close(sockfd);
-            return;
-        }
-        buffer[len] = '\0';
-        printf("Received: %s\n", buffer);
-    }
-}
-
-int main() {
-    pthread_create(&udpSend, NULL, udpSenderThread, NULL);
-    // pthread_create(&udpRecv, NULL, udpReceiverThread, NULL);
-    pthread_join(udpSend, NULL);
-    // pthread_join(udpRecv, NULL);
-}
\ No newline at end of file
diff --git a/udp2lcm.c b/udp2lcm.c
new file mode 100644
index 0000000..ec8aee8
--- /dev/null
+++ b/udp2lcm.c
@@ -0,0 +1,184 @@
+// Encoded in UTF-8
+
+/*
+ * struct path_ctrl_t
+ * {
+ *     int8_t cmd;
+ *     int8_t speed;
+ * }
+ * 这是与轮控模块通信的数据结构，speed为0-100表示速度
+ * cmd为0停，1前，2左，3右
+ */
+#include "path/path_ctrl_t.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define MAX_BUFFER_SIZE 1024
+#define PORT 5001
+
+pthread_t udpRecv, udpSend;
+int8_t heartBeat[9] = {0}; // 用于存储需要发送的心跳信息
+// clientIP用于存储手机端IP地址，仅在第一次接收消息后修改
+// 本身就是inet_ntoa(clientAddr.sin_addr)，二者内容完全一致
+struct sockaddr_in clientAddr;
+char clientIP[20] = "";
+lcm_t *path_ctrl_lcm;
+
+bool LCMInit();
+void udpSendHandler();
+void udpRecvHandler();
+void parseCmd(const char *buffer, int bytesReceived);
+
+int main() {
+    if (!LCMInit()) {
+        fprintf(stderr, "Error: Failed to initialize LCM\n");
+        return -1;
+    }
+    // 开启udp接收线程
+    pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
+    return 0;
+}
+
+bool LCMInit() {
+    // TODO: 在这里同意初始化各个LCM变量
+    // ATTENTION:
+    // 是否在这里订阅各个通道、各个通道需要在什么地方订阅，需要慎重考虑
+    if ((path_ctrl_lcm = lcm_create(NULL)) == NULL) {
+        fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
+        return false;
+    }
+
+    return true;
+}
+
+void udpRecvHandler() {
+    int socketfd = -1;
+    char buffer[MAX_BUFFER_SIZE];
+    struct sockaddr_in serverAddr;
+    socklen_t addrLen = sizeof(serverAddr);
+    int bytesReceived;
+
+    int retval;
+    struct pollfd fds;
+
+    if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
+        fprintf(stderr, "Error: Failed to create socket\n");
+        return;
+    }
+    memset((char *)&serverAddr, 0, sizeof(serverAddr));
+    memset(buffer, 0, sizeof(buffer));
+    serverAddr.sin_family = AF_INET;
+    serverAddr.sin_port = htons(port);
+    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
+        -1) {
+        fprintf(stderr, "Error: Bind failed\n");
+        close(socketfd);
+        return;
+    }
+
+    bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
+                             (struct sockaddr *)&clientAddr, &addrLen);
+    if (bytesReceived == -1) {
+        fprintf(stderr, "Error: Failed to receive data\n");
+        close(socketfd);
+        return;
+    }
+    printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
+           buffer);
+    strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
+
+    // 在接收到第一条消息之后，开启udp发送线程，用于发送心跳信息
+    if (pthread_create(&udpSend, NULL, udpSendHandler, NULL) != 0) {
+        fprintf(stderr, "Error creating udpSend thread\n");
+        close(socketfd);
+        exit(-1);
+    }
+
+    fds.fd = socketfd;
+    fds.events = POLLIN;
+
+    while (true) {
+        retval = poll(&fds, 1, 3000);
+        if (retval == -1) {
+            fprintf(stderr, "Error: select failed\n");
+            break;
+        } else if (retval == 0) {
+            printf("No data within three seconds.\n");
+            speedControl(0, 0);
+        } else {
+            bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
+                                     (struct sockaddr *)&clientAddr, &addrLen);
+            if (bytesReceived == -1) {
+                fprintf(stderr, "Error: Failed to receive data\n");
+                continue;
+            }
+            printf("Massage from client %s: ", clientIP);
+            for (int i = 0; i < bytesReceived; i++) {
+                printf("%02x ", buffer[i]);
+            }
+            printf("\n");
+            parseCmd(buffer, bytesReceived);
+        }
+    }
+}
+
+void udpSendHandler() {
+    // 创建UDP套接字
+    int sockfd = -1;
+    struct sockaddr_in serverAddr;
+    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
+        fprintf(stderr, "Error: Failed to create socket\n");
+        return;
+    }
+
+    memset((char *)&serverAddr, 0, sizeof(serverAddr));
+    serverAddr.sin_family = AF_INET;   // 设置地址族为IPv4
+    serverAddr.sin_port = htons(port); // 设置端口号
+    if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
+        fprintf(stderr, "Error: Failed to convert IP address\n");
+        close(sockfd);
+        return;
+    }
+
+    while (true) {
+        sendto(sockfd, (const char *)heartBeat, 9, 0,
+               (struct sockaddr *)&serverAddr, sizeof(serverAddr));
+        printf("Send: Data %02x udpSend successfully!\n", a);
+        sleep(1);
+    }
+}
+
+void parseCmd(const char *buffer, int bytesReceived) {
+    // TODO: 完成本函数的内容
+    if (buffer == NULL || strlen(buffer) != 9) {
+        fprintf(stderr, "Error: Invalid message\n");
+        return;
+    }
+
+    if (buffer[0] == 0) {
+        // 手机端用来与udp2lcm服务器建立连接的初始化消息
+        fprintf(stderr, "Why Init cmd here?\n");
+    } else if (buffer[0] == 1) {
+        // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
+        path_ctrl_t path;
+        path.cmd = buffer[1];
+        path.speed = buffer[2];
+        path_ctrl_t_publish(path_ctrl_lcm, "wheel_ctrl", &path);
+    } else if (buffer[0] == 2) {
+        /*
+         * 结束建图，主要有以下几个任务：
+         * - 向轮控模块发送停止命令
+         * - 通知算法模块结束建图（FIXME: 是否接受算法模块的回应？）
+         * - 拉起http服务器进程
+         * - 向手机端通知建图完成
+         * -
+         * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
+         */
+    } else if (buffer[0] == 3) {
+        // 手机发来的终点坐标，转发给算法模块
+        ;
+    }
+}
\ No newline at end of file