1 files changed, 118 insertions, 0 deletions

diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
new file mode 100644
index 0000000..fdf32c8
--- /dev/null
+++ b/udp2lcm/udp2lcm.c
@@ -0,0 +1,118 @@
+// Encoded in UTF-8
+
+/*
+ * struct path_ctrl_t
+ * {
+ *     int8_t cmd;
+ *     int8_t speed;
+ * }
+ * 这是与轮控模块通信的数据结构，speed为0-100表示速度
+ * cmd为0停，1前，2左，3右
+ */
+
+#include "udp2lcm.h"
+
+pthread_t udpRecv, udpSend;
+int8_t heartBeat[9] = {0};
+pthread_mutex_t heartBeatMutex;
+struct sockaddr_in clientAddr;
+char clientIP[20] = "";
+lcm_t *lcm;
+
+int main() {
+    if (!LCMInit()) {
+        fprintf(stderr, "Error: Failed to initialize LCM\n");
+        return -1;
+    }
+    // 初始化线程锁
+    pthread_mutex_init(&heartBeatMutex, NULL);
+    // 开启udp接收线程
+    pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
+    pid_t httpServerPid = -1;
+    if ((httpServerPid = fork()) == 0) {
+        // 拉起服务器进程，手机端需要申请的文件/mnt/cf/mapfile/defaultMap.txt
+        // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
+        chdir("/mnt/cf/mapfile/");
+        /*
+         * execlp使用系统调用exec()执行一个程序
+         * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL
+         * 其他的是传递过去的参数，按一般约定，第一个参数是程序名，不被使用
+         * 因而这里出现了两个python
+         */
+        execlp("python", "python", "-m", "http.server", NULL);
+    }
+    while (true) {
+        pause();
+    }
+    return 0;
+}
+
+bool LCMInit() {
+    if ((lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1")) == NULL) {
+        fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
+        return false;
+    }
+    return true;
+}
+
+void parseCmd(const char *buffer, int bytesReceived) {
+    if (buffer == NULL) {
+        fprintf(stderr, "Error: Invalid message\n");
+        return;
+    }
+
+    path_ctrl_t path;
+    robot_control_t robotCtrlData;
+    pose_t curPos;
+    if (buffer[0] == 0) {
+        // 手机端用来与udp2lcm服务器建立连接的初始化消息
+        // 也是手机端的心跳
+        ;
+    } else if (buffer[0] == 1) {
+        // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
+        path.cmd = buffer[1];
+        path.speed = buffer[2];
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+    } else if (buffer[0] == 2) {
+        /*
+         * 结束建图，主要有以下几个任务：
+         * - 向轮控模块发送停止命令
+         * - 通知算法模块结束建图
+         * - 拉起http服务器进程（改由main拉起，省的每次拉起浪费资源）
+         * - 向手机端通知建图完成
+         * 开始通过lcm接受算法给出的实时坐标，存储至heartBeat数组中（对应格式）
+         */
+        path.cmd = 0;
+        path.speed = 0;
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+
+        robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+
+        pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
+    } else if (buffer[0] == 3) {
+        // 手机发来的终点坐标，转发给算法模块
+    }
+}
+
+void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
+                 const pose_t *msg, void *userdata) {
+    int16_t x, y, sita;
+    x = (int16_t)msg->pos[0];
+    y = (int16_t)msg->pos[1];
+    sita = (int16_t)msg->pos[2];
+    while (sita > 180)
+        sita -= 360;
+    while (sita < -180)
+        sita += 360;
+    pthread_mutex_lock(&heartBeatMutex);
+    heartBeat[0] = 0x03;
+    heartBeat[1] = heartBeat[2] = 0;
+    heartBeat[3] = x >> 8;
+    heartBeat[4] = x;
+    heartBeat[5] = y >> 8;
+    heartBeat[6] = y;
+    heartBeat[7] = sita >> 8;
+    heartBeat[8] = sita;
+    pthread_mutex_unlock(&heartBeatMutex);
+}
\ No newline at end of file