1 files changed, 28 insertions, 17 deletions

diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index 76f41b0..9ac380f 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -30,6 +30,7 @@ int main() {
     pthread_mutex_init(&heartBeatMutex, NULL);
     // 开启udp接收线程
     pthread_create(&udpRecv, NULL, udpRecvHandler, NULL);
+    pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL);
     if ((httpServerPid = fork()) == 0) {
         // 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt
         // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改
@@ -43,7 +44,8 @@ int main() {
         execlp("python", "python", "-m", "http.server", NULL);
     }
     while (true) {
-        pause();
+        // pause();
+        lcm_handle(lcm);
     }
     return 0;
 }
@@ -91,28 +93,34 @@ void parseCmd(const char *buffer, int bytesReceived) {
         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]);
+        int16_t x, y, sita;
+        x = swapEndian(*(int16_t *)&heartBeat[3]);
+        y = swapEndian(*(int16_t *)&heartBeat[5]);
+        sita = swapEndian(*(int16_t *)&heartBeat[7]);
+        printf("NOW point: x: %d, y: %d, sita: %d\n", x, y, sita);
+        robotCtrlData.dparams[4] = (double)x / 20;
+        robotCtrlData.dparams[5] = (double)y / 20;
+        robotCtrlData.dparams[6] = (double)sita * M_PI / 180;
         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];
+        x = swapEndian(*(int16_t *)&buffer[3]);
+        y = swapEndian(*(int16_t *)&buffer[5]);
+        sita = 0;
+        // printf("End point: x: %d, y: %d, sita: %d\n", x, y, sita);
+        // printf("Original buffer: ", buffer);
         robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
-        robotCtrlData.dparams[0] = x;
-        robotCtrlData.dparams[1] = y;
+        robotCtrlData.dparams[0] = (double)x / 20;
+        robotCtrlData.dparams[1] = (double)y / 20;
         robotCtrlData.dparams[2] = sita;
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
         freeRobotCtrl(&robotCtrlData);
@@ -122,9 +130,9 @@ void parseCmd(const char *buffer, int bytesReceived) {
 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];
+    x = (int16_t)((double)msg->pos[0] * 20);
+    y = (int16_t)((double)msg->pos[1] * 20);
+    sita = (int16_t)((double)msg->pos[2] * 180 / M_PI);
     while (sita > 180)
         sita -= 360;
     while (sita < -180)
@@ -133,10 +141,13 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
     heartBeat[0] = 0x03;
     heartBeat[1] = heartBeat[2] = 0;
     heartBeat[3] = x >> 8;
-    heartBeat[4] = x;
+    heartBeat[4] = x & 0xff;
     heartBeat[5] = y >> 8;
-    heartBeat[6] = y;
+    heartBeat[6] = y & 0xff;
     heartBeat[7] = sita >> 8;
     heartBeat[8] = sita;
     pthread_mutex_unlock(&heartBeatMutex);
-}
\ No newline at end of file
+    printf("curpose: x: %d, y: %d, sita: %d\n", x, y, sita);
+}
+
+int16_t swapEndian(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); }
\ No newline at end of file