# <center>ROS和STM32通訊 Ⅰ</center>
###### <center>rosserial實作</center>
<!--### **<u>前情提要</u>**
我來自清大機器人團隊DIT Robotics，目前大二。在Eurobot2022歐洲自動化機器人比賽中，其中負責的項目之一是處理ROS和STM32的通訊。之前從未接觸過相關技術，在ROS和STM32領域也是初學者。一開始上網查資料，發現網路上很少詳細的步驟教學，資料也都零零散散，自己摸索了很久，也尋求很多人的幫助，決定寫下完整實作紀錄。</br>-->

---
### **<u>rosserial簡介</u>**
rosserial是透過在ROS端運行一節點，將訊息序列化，使得STM32可以作為一節點，透過ROS master可直接與ROS端其他節點進行通信。</br>

**我使用的是Ubuntu 20.04 ROS Noetic 搭配 STM32F429ZI 開發板。**

---
### **<u>ROS端安裝設定</u>**</br>

1. 打開終端機安裝兩個package(注意版本號)

    sudo apt-get install ros-noetic-rosserial ros-noetic-rosserial-arduino

2. 選定一個位置下載ros-drivers(注意版本號)

    git clone https://github.com/ros-drivers/rosserial.git --branch noetic-devel

3. 上一個步驟會下載名稱為rosserial的資料夾，移至該資料夾內。

    cd ~/your_directory/rosserial

4. 利用指令ls，列出資料夾內內容。其中一個package為rosserial_python，將其複製到平常使用的工作區裡的src資料夾。

    sudo cp -avr rosserial_python ~/your_ws/src

5. 完成ROS端設定！

---

### **<u>STM端安裝設定</u>(這裡使用STM32CubeIDE)**</br>

1. 在左列project selector中，選擇要使用rosserial的專案，按下右鍵選擇"Convert to C++"
2. 生成rosserial需要用到的Library
(1) 首先，移動到Ubuntu，生成Library，步驟如下。
(2) 移至平常使用的工作區裡的src資料夾。

    cd ~/your_ws/src

(3) 下載rosserial_stm32 package</br>

    git clone https://github.com/yoneken/rosserial_stm32.git

(4) 移至根目錄，進行編譯，並source一次devel資料夾裡的setup.bash。</br>

    cd ..
    catkin_make
    source devel/setup.bash

(5) 移至剛下載的package內的src內的rosserial_stm32。</br>

    cd ~/your_ws/src/rosserial_stm32/src/rosserial_stm32

(6) make_libraries文件內容需修改，**<font color=red>print需加上前後括號</font>**。</br>

    sudo nano make_libraries.py

<div style="text-align: center">
<img src="https://i.imgur.com/O0XeiOe.png"/>
</div>

(7)在package內的src建立一資料夾，命名為Inc(不要自行更改資料夾名稱)。這裡我選擇在rosserial_stm32這個package。

    cd ~/your_ws/src/rosserial_stm32
    mkdir Inc

(8) 生成Library，後面接上Library生成位置。這裡生成在上步驟建立的Inc資料夾內。</br>

    rosrun rosserial_stm32 make_libraries.py ~/your_ws/src/rosserial_stm32

(9) 會出現兩個選擇，選擇執行在src資料夾內的 make_libraries.py，通常為2。</br>
(10) 移至package內的src內的Inc，使用ls指令列出資料夾內容，可以看到生成的Library會放在這裡。</br>

    cd ~/your_ws/src/rosserial_stm32/Inc

3. 將上步驟生成的Library，移至/你的STM專案資料夾/Core/Inc內(直接複製進資料夾)，並回到STM32CubeIDE中，在左列project selector中，選擇該專案，按下右鍵選擇"Refresh"，按下下拉選單可以看見Library已被成功加入。
<div style="text-align: center">
<img src="https://i.imgur.com/IFtVdRa.png"/>
</div>
4. 設置ioc檔
(1) 配置UART腳位。這裡選擇USART3，TX腳位為PB10，RX腳位為PD9。
(2) 選取左列Connectivity USART3。
&emsp;● Mode：Asynchronous。
&emsp;● NVIC：勾選 USART3 global interrupt。
&emsp;● DMA：Add USART3_TX 和 USART3_RX，將 priority 設為 High。
&emsp;● parameter settings：115200 Bits/s (自行設定鮑率)。
(3) 選取左列System Core裡的RCC，將HSE選為BYPASS Clock Source。
(4) 設定Clock Configuration。
<div style="text-align: center">
<img src="https://i.imgur.com/3OIYpzd.png"/>
</div>
5. 在Core/src新增一個檔案mainpp.cpp，並在main.c 中 include mainpp.h，將函式setup和loop寫進main.c。
**main.c**

    int main(void){
    ...
      MX_DMA_Init();
      MX_USART3_UART_Init();
      /* USER CODE BEGIN 2 */
      setup();
      /* USER CODE END 2 */

      /* Infinite loop */
      /* USER CODE BEGIN WHILE */
      while (1)
      {
        /* USER CODE END WHILE */

        /* USER CODE BEGIN 3 */
        loop();
      }
      /* USER CODE END 3 */
    ...

**mainpp.cpp**（這裡的寫法和在ROS發佈、訂閱topic，基本上概念差不多。）

    #include "mainpp.h"
    #include "ros.h"
    #include "geometry_msgs/Twist.h"

    void vel_callback(const geometry_msgs::Twist &msg)
    {
       vel[0] = msg.linear.x;  //double vel[3]宣告在mainpp.h
       vel[1] = msg.linear.y;
       vel[2] = msg.angular.z;
    }

    ros::NodeHandle nh;
    ros::Subscriber<geometry_msgs::Twist> sub("give_car_speed", vel_callback);

    void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
    {
        nh.getHardware()->flush();
    }
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        nh.getHardware()->reset_rbuf();
    }
    void setup(void)
    {
        nh.initNode();
        nh.subscribe(sub);
    }
    void loop(void)
    {
        nh.spinOnce();
    }

**mainpp.h**

    /*
     * mainpp.h
     *
     *  Created on: 2018/01/17
     *      Author: yoneken
     */

    #ifndef MAINPP_H_
    #define MAINPP_H_

    #ifdef __cplusplus
    extern "C"
    {
    #endif
        
    void setup(void);
    void loop(void);
    double vel[3];
        
    #ifdef __cplusplus
    }
    #endif

    #endif /* MAINPP_H_ */

6. 在Core/Inc裡修改STM32HardWare.h。
(1) 選擇使用的板子型號。
(2) 選擇使用的UART。
<div style="text-align: center">
<img src="https://i.imgur.com/ak9uK5k.png"/>
</div>
</br>

:::info
**小補充**

&emsp;&emsp;如果你的板子型號不在他原本預設的裡面，可以自己改寫成以下幾行。**假設我使用的開發板型號是STM32H723ZG。**

    #define STM32H7xx  //上圖 Line 38
    ...
    #ifdef STM32H7xx
    #include "stm32H7xx_hal.h"
    #include "stm32H7xx_hal_uart.h"
    #endif /* STM32H7xx */

:::
7. 完成STM端設定！</br>

---
### **<u>開始測試rosserial</u>**</br>

1. **硬體配置**

<div style="text-align: center">
<img src="https://i.imgur.com/kDysIuI.png"/>
</div>

1. **開始測試**

    rosrun rosserial_python serial_node.py _baud:=115200

如果成功，會顯示以下畫面。列出Baud Rate，發佈和訂閱的topic name，以及buffer size。
<div style="text-align: center">
<img src="https://i.imgur.com/OQJA2vu.png"/>
</div></br>

恭喜，可以開始將rosserial應用在自己的專案了！

:::info
一般會將STM端的publish寫進中斷，比較好控制發佈頻率，之後會再寫一篇介紹如何使用中斷功能搭配UART通訊！</br>
:::

---
### **<u>DEBUG</u>**</br>

1. **CP2102 USB to TTL port問題**
USB不一定會連接在預設的/dev/ttyUSB0下，可以利用以下指令檢查。</br>

    dmesg | grep tty

假設USB連接在/dev/ttyUSB1下，執行指令時可以加上port。</br>

    rosrun rosserial_python serial_node.py _baud:=115200 _port:=/dev/ttyUSB1

權限不足時，可以輸入以下指令開啟port權限。</br>

    sudo chmod 777 /dev/ttyUSB0

1. **ioc檔問題**
我遇到一個比較奇怪的問題是，ioc檔儲存後自動生成的程式碼有錯誤。結論是DMA的init需在UART的init之前，我的自動生成會顛倒。</br>

    MX_DMA_Init();
    MX_USART3_UART_Init();

1. **硬體問題1**
開發板上的TX接到CP2102的RX，開發板上的RX接到CP2102的TX。
1. **硬體問題2**
我是使用VMWare虛擬機開啟Ubuntu。若錯過USB選擇連接的視窗，需自行將CP2102 USB to TTL連上虛擬機。
選擇右上角Player -> Removable Devices -> Silicon CP2102 USB to UART Bridge Controller -> Connect。
1. **Error code-1**
Error code：Tried to publish before configured, topic id 125
<div style="text-align: center">
<img src="https://i.imgur.com/cibyAvC.png"/>
</div>
解決方法：RESET STM32。
1. **Error code-2**
Error code：message larger than buffer</br>
解決方法：調整buffer size。</br>
(1)修改STMHardware.h (位置：/Core/Inc/STMHardware.h)

    ...
    class STM32Hardware {
      protected:
        UART_HandleTypeDef *huart;

        const static uint16_t rbuflen = 1024; // 512 to 1024
        uint8_t rbuf[rbuflen];
        uint32_t rind;
        inline uint32_t getRdmaInd(void){ return (rbuflen - __HAL_DMA_GET_COUNTER(huart->hdmarx)) & (rbuflen - 1); }

        const static uint16_t tbuflen = 1024; // 512 to 1024
        uint8_t tbuf[tbuflen];
        uint32_t twind, tfind;
    ...

(2)修改node_handle.h (位置：/Core/Inc/ros/node_handle.h)

    ...
    template<class Hardware,
             int MAX_SUBSCRIBERS = 25,
             int MAX_PUBLISHERS = 25,
             int INPUT_SIZE = 1024,   // 512 to 1024
             int OUTPUT_SIZE = 1024>  // 512 to 1024
    ...

---
下一篇：[ROS和STM32通訊 Ⅱ - rosserial搭配STM中斷功能](https://hackmd.io/@JINGCCC/rosserial_2)

---
<h5 style="text-align:right">寫於2022/1/28</h5>