1 files changed, 40 insertions, 16 deletions

diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index fdf32c8..76f41b0 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -1,16 +1,6 @@
 // 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"
+#include <sys/signal.h>
 
 pthread_t udpRecv, udpSend;
 int8_t heartBeat[9] = {0};
@@ -19,7 +9,19 @@ struct sockaddr_in clientAddr;
 char clientIP[20] = "";
 lcm_t *lcm;
 
+pid_t httpServerPid = -1;
+
+void sigIntHandler(int sig) {
+    if (httpServerPid != -1) {
+        kill(httpServerPid, SIGINT);
+    }
+    printf("Exiting...\n");
+    exit(sig);
+}
+
 int main() {
+    // 当遇到Ctrl-C时，递归地杀死所有子进程
+    signal(SIGINT, sigIntHandler);
     if (!LCMInit()) {
         fprintf(stderr, "Error: Failed to initialize LCM\n");
         return -1;
@@ -28,11 +30,10 @@ int main() {
     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
+        // 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt
         // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
-        chdir("/mnt/cf/mapfile/");
+        chdir("/data/test");
         /*
          * execlp使用系统调用exec()执行一个程序
          * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL
@@ -48,7 +49,8 @@ int main() {
 }
 
 bool LCMInit() {
-    if ((lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1")) == NULL) {
+    // 多播地址范围：224.0.0.0~239.255.255.255
+    if ((lcm = lcm_create(NULL)) == NULL) {
         fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n");
         return false;
     }
@@ -86,12 +88,34 @@ void parseCmd(const char *buffer, int bytesReceived) {
         path.speed = 0;
         path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
 
+        printf("Save Map!\n");
         robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
-
         pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
+        freeRobotCtrl(&robotCtrlData);
+
+        sleep(2);
+
+        robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
+        pthread_mutex_lock(&heartBeatMutex);
+        robotCtrlData.dparams[4] = (double)(heartBeat[3] << 8 + heartBeat[4]);
+        robotCtrlData.dparams[5] = (double)(heartBeat[5] << 8 + heartBeat[6]);
+        robotCtrlData.dparams[6] = (double)(heartBeat[7] << 8 + heartBeat[8]);
+        pthread_mutex_unlock(&heartBeatMutex);
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     } else if (buffer[0] == 3) {
         // 手机发来的终点坐标，转发给算法模块
+        int x, y, sita;
+        x = buffer[3] << 8 | buffer[4];
+        y = buffer[5] << 8 | buffer[6];
+        sita = buffer[7] << 8 | buffer[8];
+        robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
+        robotCtrlData.dparams[0] = x;
+        robotCtrlData.dparams[1] = y;
+        robotCtrlData.dparams[2] = sita;
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     }
 }