# <center>ROS和STM32通訊 Ⅲ</center>
###### <center>rosserial改良</center>
<!--### **<u>前情提要</u>**
我來自清大機器人團隊DIT Robotics，目前大二。在Eurobot2022歐洲自動化機器人比賽中，其中負責的項目之一是處理ROS和STM32的通訊。-->

&emsp;&emsp;之前寫過兩篇文章，介紹如何使用rosserial讓ROS和STM32能夠成功通訊，以及STM搭配中斷功能使用，可以點下面連結前往閱讀。今天的文章主要是記錄使用後修改的一些地方。</br>

&emsp;&emsp;第一篇：[ROS和STM32通訊 Ⅰ - rosserial實作](https://hackmd.io/@JINGCCC/rosserial_1)

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

---

### **<u>前情提要</u>**

&emsp;&emsp;在使用rosserial的過程中，單純在開發板測試時都沒有出現問題，但移上去機器後問題不斷：使用過程中斷線、一連上就斷線、有一陣子完全無法連上，狀況不斷，後來在硬體、軟體上都做了改良，使用下來的結果也很穩定。(成果都是學長[@sunfu_chou](https://github.com/sunfu-chou/rosserial)做的，我只是記錄下來><)

---

### **<u>開始實作</u>**
#### **[硬體]**
&emsp;&emsp;我們原本使用的線是自己壓的杜邦線，簡單用蛇管纏起來而已，但因為走線繞過很多馬達，推測當馬達瞬間輸出較大時，可能會產生磁場干擾通訊，後來換成隔離線，像是下圖這種線的外面有包鋁箔的，同樣自己壓端子。

<center>
<img src="https://i.imgur.com/dWeKZ9T.jpg" style="zoom: 45%;"/>
<div>● 隔離線</div>
</center>

&emsp;&emsp;
&emsp;&emsp;**方法一：** 原先的接法是透過CP2102 USB to TTL從STM32連接到派上的USB插座(如下圖)。為了在電路中減少出問題的因子，我們不使用USB to TTL，而是將STM32上的TX、RX直接與樹莓派上的GPIO接口TX、RX對接。
<center>
<div style="text-align: center">
<img src="https://i.imgur.com/kDysIuI.png"/>
</div>
<div>● 以前的接法</div>
</center>

&emsp;&emsp;

<center>
<div style="text-align: center">
<img src="https://i.imgur.com/Q4fmVLk.png" style="zoom:45%;"/>
</div>
<div>● 樹莓派 pinout (資料來源：<a href="http://pinout.xyz">http://pinout.xyz</a>)</div>
</center>

&emsp;&emsp;
&emsp;&emsp;**方法二：** 後來又覺得，自己壓的端子，也可能是讓通訊不穩定的因素之一，因此後來直接使用市面上販售的USB-A to micro-USB(品質較統一)將STM32與PI連接。(這種方法不用改程式，唯一需注意板子上的接口是使用哪一個UART。)

<center>
<div style="text-align: center" >
<img src="https://i.imgur.com/rjhGTeP.jpg" style="zoom: 10%;"/>
</div>
<div>● STM32 micro-USB接口</div>
</center>

&emsp;&emsp;
&emsp;&emsp;以上兩種方法都有經過長時間測試，表現也都還蠻穩定的。

#### **[ROS端]**
1. 之前在[ROS和STM32通訊 Ⅰ](https://hackmd.io/@JINGCCC/rosserial_1) 中<ins>ROS端安裝設定</ins>步驟四是將 rosserial_python 複製到自己的工作區，後來我們換成使用 rosserial_server ( rosserial 的cpp版本，一樣在rosserial的資料夾裡)。

    sudo cp -avr rosserial_server ~/your_ws/src

2. 並且改動裡面的程式碼：
file path：~/rosserial_server/include/rosserial_server/session.h
可以搜尋 "shutdown"，大約在原來的150行左右，加入下面的程式碼。

    ...
     void shutdown()
      {
        //copy start here
        std::cout << "Sending Tx Stop Request..." << std::endl;
        uint8_t overhead_bytes = 8;
        uint16_t length = overhead_bytes;
        BufferPtr buffer_ptr(new Buffer(length));
        ros::serialization::OStream stream(&buffer_ptr->at(0), buffer_ptr->size());
        stream << (uint16_t)0xfeff << (uint16_t)0x0000 << (uint16_t)0x0bff << (uint16_t)0xf400 ;
        boost::asio::async_write(socket_, boost::asio::buffer(*buffer_ptr),
        boost::bind(&Session::write_completion_cb, this, boost::asio::placeholders::error, buffer_ptr));
        std::cout << "Stop..." << std::endl;
        //copy end here
    
         if (is_active())
        {
          stop();
        }
    ...

3. **改動原因**
我們使用邏輯分析儀分析TX、RX的訊號，先來觀察下面的這兩張圖。結果一、二的上列為ROS傳給STM的訊號，下列為STM傳給ROS的訊號。
&emsp;&emsp;
**Time Synchronization** 下列(STM32)請求現在的時間，上列(樹莓派)回傳 empty [time messsage](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Time.html)，以計算clock offset。

<center>
<div style="text-align: center" >
<img src="https://i.imgur.com/DUmLaTJ.png"/>
</div>
<div>● 結果一 — Good</div>
</center>

<center>
<div style="text-align: center" >
<img src="https://i.imgur.com/vBcjql7.png"/>
</div>
<div>● 結果二 — Bad</div>
</center>

結果一的上列是ROS傳給STM32的訊號，紅色框框是當ROS端將通訊程式停止時會發出的訊號，在正常的狀況下，當STM32收到該訊號後便停止request現在時間。

我們再看看結果二，當我們停止通訊程式時，ROS端沒有return了，但不知道什麼原因並未傳出停止通訊的訊號，可以看到STM端並不知道通訊已經終止了，仍在不斷的向樹莓派請求現在時間。

在這種情況下，進行下一次的通訊，就很有可能會導致通訊失敗。

#### **[STM32]**
1. 開啟STM專案，並打開ioc檔。將DMA的設定改為circular。
<div style="text-align: center">
<img src="https://i.imgur.com/UFxghCK.png"/>
</div>

2. 原本寫在 main.cpp 裡的 RxCallback 刪掉。

    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        nh.getHardware()->reset_rbuf();
    }

3. 新增 ErrorCallback，將 huartX 改成自己使用的。不知道要寫在哪裡的話，寫在跟中斷 Callback 一樣的位置就沒錯了！

    void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) 
    {
        if (huart == &huart3) 
        {
            Vx = 0.0;
            Vy = 0.0;
            w_car = 0.0;
            HAL_UART_DeInit(&huart3);
            MX_USART3_UART_Init();
            nh.getHardware()->init();
        }
    }

---

參考資料：
1. [Rosserial/Overview/protocal](http://wiki.ros.org/rosserial/Overview/Protocol)

---

<h5 style="text-align:right">寫於2022/12/2</h5>
<h5 style="text-align:right">最終完成於2023/2/16</h5>