Fix linking error caused by wrong lib pathHEADmaster

In the old commit, CMakeLists is set to find liblcm in
${CMAKE_SOURCE_ROOT}/libs, and there are two dirs: aarch64 and x86.
When I compile it for x86-64 computer using the cmake on host machine,
it will automatically find lib under ${CMAKE_SOURCE_ROOT}/lib/x86, and
when compiling for arm using cmake/compiler provided by ohos, it will
also find under ${CMAKE_SOURCE_ROOT}/lib/aarch64. Sounds good.

But, this is just a feature of special version for cmake/make/binutils,
causing error on other linux distributions. The path should be given,
but tools wouldn't find libs there recursively, which means path should
be spercific.

So in this commit, I use runme.sh to compile. It will work depending on
the argument(named arch) given by user, and pass it as a macro named
OHOS_ARCH to cmake, which is used in linking path of CMakeLists.

Besides, libpthread.so is not always default linked. So let's write it
in CMakeLists.

Last but not least, keeping source code clean is our duty. There's a
.clang-format under ${CMAKE_SOURCE_ROOT} to help. Use it.

Moving for the better!

14 files changed, 264 insertions, 30 deletions

diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..a1054a5
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,121 @@
+---
+Language:        Cpp
+# BasedOnStyle:  LLVM
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Right
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: MultiLine
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:   
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     80
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:   
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Preserve
+IncludeCategories: 
+  - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority:        2
+  - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
+    Priority:        3
+  - Regex:           '.*'
+    Priority:        1
+IncludeIsMainRegex: '(Test)?$'
+IndentCaseLabels: false
+IndentPPDirectives: None
+IndentWidth:     4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 60
+PointerAlignment: Right
+ReflowComments:  true
+SortIncludes:    true
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles:  false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Cpp11
+StatementMacros: 
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        8
+UseTab:          Never
+...
+
diff --git a/.gitignore b/.gitignore
index 938c377..9497a1d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 build/*
 bin/*
-.vscode/*
\ No newline at end of file
+.vscode/*
+compile_commands.json
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c487036..c01ccfc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,10 +9,10 @@ include_directories(
 )
 # 添加库文件路径
 link_directories(
-    ${CMAKE_SOURCE_DIR}/lib
+    ${CMAKE_SOURCE_DIR}/lib/${OHOS_ARCH}
 )
 # 设置输出路径
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
 
 add_subdirectory(serial)
-add_subdirectory(udp2lcm)
\ No newline at end of file
+add_subdirectory(udp2lcm)
diff --git a/lib/aarch64/liblcm.so b/lib/arm64-v8a/liblcm.so
index 98bb954..98bb954 100644..100755
--- a/lib/aarch64/liblcm.so
+++ b/lib/arm64-v8a/liblcm.so
diff --git a/lib/x86/liblcm.so b/lib/x86_64/liblcm.so
index cb18db8..cb18db8 100755
--- a/lib/x86/liblcm.so
+++ b/lib/x86_64/liblcm.so
diff --git a/runme.sh b/runme.sh
new file mode 100755
index 0000000..947d50a
--- /dev/null
+++ b/runme.sh
@@ -0,0 +1,34 @@
+#!/bin/bash
+
+##########################################################################
+# File Name    : compile.sh
+# Encoding     : utf-8
+# Author       : We-unite
+# Email        : weunite1848@gmail.com
+# Created Time : 2024-02-29 15:19:15
+##########################################################################
+
+set -e
+
+if [ $UID -eq 0 ]; then
+        echo "Please do not run this script as root"
+        exit 1
+fi
+
+if [ $# -ne 1 ]; then
+        echo "ERROR: $0 <arm64-v8a|x86_64>"
+        exit 1
+fi
+
+arch=$1
+link=static # or shared
+native_path=/home/player/app/native
+
+if [ $arch == "arm64-v8a" ]; then
+        export PATH=$native_path/build-tools/cmake/bin:$PATH
+        cmake -B build -D OHOS_STL=c++_$link -D OHOS_ARCH=$arch -D OHOS_PLATFORM=OHOS -D CMAKE_TOOLCHAIN_FILE=$(find $native_path -name ohos.toolchain.cmake)
+else
+        cmake -B build -D OHOS_ARCH=$arch
+fi
+
+cmake --build build
diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt
index 5ded98c..c0bc05a 100644
--- a/serial/CMakeLists.txt
+++ b/serial/CMakeLists.txt
@@ -8,4 +8,4 @@ aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS)
 add_executable(serial ${DIR_SRCS})
 
 # 添加链接库
-target_link_libraries(serial lcm)
\ No newline at end of file
+target_link_libraries(serial lcm pthread)
diff --git a/serial/serial.c b/serial/serial.c
index a2f253a..6163703 100644
--- a/serial/serial.c
+++ b/serial/serial.c
@@ -1,7 +1,12 @@
 #include "serial.h"
+#include <math.h>
 
 lcm_t *lcm;
 
+// TODO 屏蔽停止指令
+bool stopFlag = false;
+////////////////////////
+
 int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度
 double curOmega = 0.0;
 // 临界区数据
@@ -72,6 +77,23 @@ void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
         curOmega = (double)-DEFAULT_SPEED * FULLSPEED / 100 / RADIUS;
         curSpeed = 0;
         break;
+    // TODO 屏蔽停止指令
+    case 4:
+        wheelSend(0x00, 0x00, 0x00, 0x00);
+        curOmega = 0;
+        curSpeed = 0;
+        stopFlag = true;
+        printf("Stop for 2 seconds\n");
+        break;
+    // 取消屏蔽
+    case 5:
+        wheelSend(0x00, 0x00, 0x00, 0x00);
+        curOmega = 0;
+        curSpeed = 0;
+        stopFlag = false;
+        printf("Stop but can recieve cmd\n");
+        break;
+    ////////////////////////////////////
     case 0:
     default:
         wheelSend(0x00, 0x00, 0x00, 0x00);
@@ -85,6 +107,11 @@ void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
 
 void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
                const path_t *msg, void *userdata) {
+    // TODO
+    if (stopFlag) {
+        return;
+    }
+    /////////////////
     int16_t length = msg->length;
     double v, w;
     int8_t vWheels[2];
@@ -127,7 +154,7 @@ void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
     pthread_mutex_unlock(&curPoseMutex);
 }
 
-void sendCurPose() {
+void *sendCurPose(void *args) {
     pose_t pose;
     while (true) {
         renewCurPose();
@@ -152,4 +179,4 @@ void renewCurPose() {
     curPose[2] += curOmega * dt;
     lastTime = clock();
     pthread_mutex_unlock(&curPoseMutex);
-}
\ No newline at end of file
+}
diff --git a/serial/serial.h b/serial/serial.h
index 3563557..2fbf870 100644
--- a/serial/serial.h
+++ b/serial/serial.h
@@ -19,7 +19,7 @@
 // TODO: 两个待测数据
 #define RADIUS 0.13    // 两轮之间的距离的一半，米
 #define FULLSPEED 0.60 // 轮子全速，m/s
-#define fabs(x) ((x) > 0 ? (x) : -(x))
+// #define fabs(x) ((x) > 0 ? (x) : -(x))
 typedef unsigned char byte;
 
 int identify_device(const char *port);
@@ -34,6 +34,6 @@ void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
 void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
                 const pose_t *msg, void *userdata);
 void renewCurPose();
-void sendCurPose();
+void *sendCurPose(void *args);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/serial/wheel.c b/serial/wheel.c
index 8155734..c470234 100644
--- a/serial/wheel.c
+++ b/serial/wheel.c
@@ -5,7 +5,8 @@ char *portname; // 串口设备名
 struct termios tty;
 
 bool whellInit() {
-    portname = findUSBDev("wheel");
+    // portname = findUSBDev("wheel");
+    portname = "/dev/ttyUSB1"; // "/dev/ttyUSB1
     if (portname == NULL) {
         fprintf(stderr, "Error: Failed to find wheel serial port\n");
         return false;
@@ -55,6 +56,10 @@ bool wheelSend(byte a, byte a_v, byte b, byte b_v) {
     unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00};
     byte checksum = 0;
     data[2] = a;
+    // TODO: 速度转换
+    int temp = ((int)a_v + 2);
+    a_v = (byte)temp;
+    ////////////////////////
     data[3] = a_v;
     data[4] = b;
     data[5] = b_v;
diff --git a/udp2lcm/CMakeLists.txt b/udp2lcm/CMakeLists.txt
index aaf54db..206a8a8 100644
--- a/udp2lcm/CMakeLists.txt
+++ b/udp2lcm/CMakeLists.txt
@@ -7,4 +7,4 @@ aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS)
 
 # 生成可执行文件
 add_executable(udp2lcm ${DIR_SRCS})
-target_link_libraries(udp2lcm lcm)
\ No newline at end of file
+target_link_libraries(udp2lcm lcm pthread)
diff --git a/udp2lcm/udp.c b/udp2lcm/udp.c
index 6c4bd4a..97fb53e 100644
--- a/udp2lcm/udp.c
+++ b/udp2lcm/udp.c
@@ -1,6 +1,6 @@
 #include "udp2lcm.h"
 
-void udpRecvHandler() {
+void *udpRecvHandler(void *args) {
     int socketfd = -1;
     char buffer[MAX_BUFFER_SIZE];
     struct sockaddr_in serverAddr;
@@ -13,7 +13,7 @@ void udpRecvHandler() {
     printf("udpRecvHandler\n");
     if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
         fprintf(stderr, "Error: Failed to create socket\n");
-        return;
+        return NULL;
     }
     memset((char *)&serverAddr, 0, sizeof(serverAddr));
     memset(buffer, 0, sizeof(buffer));
@@ -25,7 +25,7 @@ void udpRecvHandler() {
         -1) {
         fprintf(stderr, "Error: Bind failed\n");
         close(socketfd);
-        return;
+        return NULL;
     }
 
     bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
@@ -33,7 +33,7 @@ void udpRecvHandler() {
     if (bytesReceived == -1) {
         fprintf(stderr, "Error: Failed to receive data\n");
         close(socketfd);
-        return;
+        return NULL;
     }
     printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
            buffer);
@@ -57,13 +57,13 @@ void udpRecvHandler() {
     robotCtrlData.iparams[0] = 1;
     robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
     freeRobotCtrl(&robotCtrlData);
-    // 随即下达30号命令，开始建图
-    robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
-    robotCtrlData.dparams[0] = 0.05;
-    robotCtrlData.niparams = 1;
-    robotCtrlData.iparams[0] = 1;
-    robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
-    freeRobotCtrl(&robotCtrlData);
+    // // 随即下达30号命令，开始建图
+    // robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
+    // robotCtrlData.dparams[0] = 0.05;
+    // robotCtrlData.niparams = 1;
+    // robotCtrlData.iparams[0] = 1;
+    // robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+    // freeRobotCtrl(&robotCtrlData);
 
     /*
      * 开始接收来自手机端的命令
@@ -99,13 +99,13 @@ void udpRecvHandler() {
     }
 }
 
-void udpSendHandler() {
+void *udpSendHandler(void *args) {
     // 创建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;
+        return NULL;
     }
 
     memset((char *)&serverAddr, 0, sizeof(serverAddr));
@@ -114,7 +114,7 @@ void udpSendHandler() {
     if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
         fprintf(stderr, "Error: Failed to convert IP address\n");
         close(sockfd);
-        return;
+        return NULL;
     }
 
     while (true) {
@@ -124,4 +124,4 @@ void udpSendHandler() {
         pthread_mutex_unlock(&heartBeatMutex);
         sleep(1);
     }
-}
\ No newline at end of file
+}
diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index 36100da..5514b24 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -71,7 +71,13 @@ void parseCmd(const char *buffer, int bytesReceived) {
     if (buffer[0] == 0) {
         // 手机端用来与udp2lcm服务器建立连接的初始化消息
         // 也是手机端的心跳
-        ;
+        // 下达30号命令，开始建图
+        robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
+        robotCtrlData.dparams[0] = 0.05;
+        robotCtrlData.niparams = 1;
+        robotCtrlData.iparams[0] = 1;
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     } else if (buffer[0] == 1) {
         // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
         path.cmd = buffer[1];
@@ -125,7 +131,47 @@ void parseCmd(const char *buffer, int bytesReceived) {
     } else if (buffer[0] == 4) {
         // 取消导航，删除上一个目标点
         robotCtrlInit(&robotCtrlData, 0, 23, 0, 0, 0, 0, 0);
+        // TODO 第一次停止指令
+        printf("Cancel Navigation!\n");
+        path.cmd = 4;
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+        /////////////////////////////
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        // TODO 再次下达停止指令
+        sleep(2);
+        path.cmd = 5;
+        printf("Stop but can recieve cmd\n");
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+        /////////////////////////
+    } else if (buffer[0] == 5) {
+        /*
+        加载地图，需要注意的是所加载的地图是由电脑控制端下发的
+        该部分由建图部分修改
+        */
+        path.cmd = 0;
+        path.speed = 0;
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+
+        printf("Load Map!\n");
+        // robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
+        // robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        // freeRobotCtrl(&robotCtrlData);
+
+        // sleep(2);
+
+        robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
+        pthread_mutex_lock(&heartBeatMutex);
+        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] = 0;
+        robotCtrlData.dparams[5] = 0;
+        robotCtrlData.dparams[6] = 0;
+        pthread_mutex_unlock(&heartBeatMutex);
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     } else {
         fprintf(stderr, "Error: Invalid command: ");
         for (int i = 0; i < 9; i++) {
diff --git a/udp2lcm/udp2lcm.h b/udp2lcm/udp2lcm.h
index 1ab7cf3..e78bc73 100644
--- a/udp2lcm/udp2lcm.h
+++ b/udp2lcm/udp2lcm.h
@@ -33,8 +33,8 @@ extern char clientIP[20];
 extern lcm_t *lcm;
 
 bool LCMInit();
-void udpSendHandler();
-void udpRecvHandler();
+void *udpSendHandler(void *args);
+void *udpRecvHandler(void *args);
 void parseCmd(const char *buffer, int bytesReceived);
 void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
                    int8_t commandid, int8_t robotid, int8_t ndparams,
@@ -44,4 +44,4 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
                  const pose_t *msg, void *userdata);
 int16_t swapEndian(int16_t val);
 
-#endif
\ No newline at end of file
+#endif