###### tags: `東京威力 TEL`

# III. 樹梅派 Raspberry PI 4B

## <font color = "orange">03. ROS x STM</font>
> 在 [$\LaTeX$ 筆記](https://www.overleaf.com/read/zgrytgmwhcqp) 的 Chapter 2 - Raspberry PI Communication 中有寫過了，是參(抄)考(襲) [陳靜學姊的筆記](https://hackmd.io/@JINGCCC/rosserial_1)。
> 不過還是自己寫一次 HackMD 版本好了 OwO

:::info
rosserial 的 package 有 rosserial_python 和 rosserial_server 兩種。
前者是 python 撰寫，後者是 cpp。
我的感想是 python 版本方便測試，而 rosserial_server 則ㄕ比較穩定。
:::

### <font color = "pink">3-1. ROS 端設定</font>

<font color = "yellow">1. 下載 package </font>

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

<font color = "yellow">2. 在 ${catkin_ws}/src 資料夾中 git clone rosserial 的資源包。 </font>

cd ${catkin_ws}/src
git clone https://github.com/ros-drivers/rosserial.git --branch noetic-devel

<font color = "yellow">3. 將 rosserial/rosserial_python 移至 ${catkin_ws}/src。 </font>
左圖移動前，右圖移動後。

 $\;\;\;$=> 

:::warning
※ 真正通訊時，只需要選一種，看是要 python 或 cpp 版本。
※ 初期可以先測試兩種通訊，看自己喜歡哪種。
:::

<font color = "yellow">4. 自建新的 package，寫入測試用程式 `stm_communication.cpp`。 </font>

這邊省略後續的編譯過程，應該都很熟悉了。
::: spoiler 程式碼 -- stm_communication.cpp

/** stm_communication.cpp **/
#include <iostream>
#include "ros/ros.h"
#include <std_msgs/Int64.h>

using namespace std;

int main(int argc, char** argv){
    ros::init(argc, argv, "stm_communication");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<std_msgs::Int64>("counting", 1);

    std_msgs::Int64 hello;
    hello.data = 0;

    while(ros::ok()){
        pub.publish(hello);
        hello.data++;
        ros::Duration(0.5).sleep();
    }

    ROS_INFO("== Terminate ! ==");
    return 0;
}

:::

### <font color = "pink">3-2. STM 端設定</font>

<font color = "yellow">1. 選「New STM32 Project」，語言選擇「C++」。 </font>


<font color = "yellow">2. 選取左列Connectivity USART3。 </font>

:::spoiler USART 接口選擇
:::warning
* **可以選 USART 其他接口**，不一定要是 3。
* DIT 的邏輯版只有 **USART1** 和 **USART3**。
* 不同 USART 的接口對應腳位不一樣，務必事先規劃好。
* 所有 USART 都要套用以下設定。
:::

需要調整的設定（全部設定參考以下）：
* <font color = "magenza">Mode：</font>Asynchronous。
* <font color = "magenza">DMA Settings：</font>Add USART3_TX 和 USART3_RX，將 priority 設為 High。
* <font color = "magenza">NVIC Settings：</font>勾選 USART3 global interrupt。
* <font color = "magenza">Parameter settings：</font>115200 Bits/s （自行設定）。

<font color = "yellow">3. STM：調 ioc。 System Core -> DMA -> USART1_RX -> Circular。</font>


<font color = "yellow">4. 選取左列 System Core 裡的RCC，將 HSE 選為 BYPASS Clock Source。 </font>


:::warning
**注意：**
如果是用 DIT 的邏輯版，HSE 改選為外部晶振「Crystal/Ceramic Resonator」。
:::

<font color = "yellow">5. 設定Clock Configuration。 </font>



<font color = "yellow">6. 下載 Library。</font>
:::spoiler 這檔案怎麼來的 ?
參考影片：[Interfacing STM32 boards to ROS using rosserial protocol](https://www.youtube.com/watch?v=cq0HmKrIOt8&t=680s&ab_channel=LearnembeddedsystemswithGeorge)
:::

下載 [雲端](
https://drive.google.com/drive/folders/1j_iB8L6iR58N32y-ehP-o2JRwnhKRFhs) 上的 Inc 資料夾，記得解壓縮。

<font color = "yellow">7. 將 Inc 資料夾丟進專案內。</font>

先複製/剪下 Inc 資料夾，進入到下圖中的資料夾層級後，直接貼上。
`~/你的STM專案資料夾/Core`（它會自動合併，不用擔心覆蓋原檔案。）


<font color = "yellow">8. 回 STM32 CubeIDE，對 Inc 資料夾點右鍵選 Refresh。</font>


應該會刷新一排跟 ROS 有關的新檔案喔！

<font color = "yellow">9. 在Core/Inc 裡修改 STM32HardWare.h。</font>

修改下圖中的三個地方。
第一個框框：修改成自己板子型號的「兩前綴字母」即可。（我是 H7）


:::warning
注意 USART3 是根據 ioc 時所配置的 USART 接口編號決定。
:::

<font color = "yellow">10. 在 Core/src/main.c 中寫進 `setup()` 和 `loop()` 和 變數 `count`。</font>
:::spoiler 程式碼1 -- 變數 count

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int count;
/* USER CODE END 0 */
// ...
:::
:::spoiler 程式碼2 -- setup() 和 loop()

int main(void)
{
...
/* Initialize all configured peripherals */
MX_GPIO_Init();
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 */
}

}

:::

:::spoiler 務必檢查： MX_DMA_Init() 順序
:::warning
MX_DMA_Init() 如果在 MX_USART3_UART_Init() 下方，要手動修改順序成上圖的順序。
:::

$\;$

<font color = "yellow">11. 在 Core/src 新增一個檔案 mainpp.cpp，複製下方範例。</font>

:::spoiler 程式碼大略解釋
:::info
跟 ROS 寫法很像，我們定義了一個 **subscriber** 接收 **counting** 這個 topic。
並定義了收到訊息後，要傳進 **count** 變數中。

**setup()** 函式就像在 Arduino 中，負責初始化，只會執行一次。
**loop()** 會一直執行，藉由 **spinOnce()** 更新 topic 訊息。
:::

:::spoiler 程式碼 -- mainpp.cpp

#include "mainpp.h"
#include "ros.h"
#include "std_msgs/Int64.h"
#include "STM32Hardware.h"

int count;

void callback(const std_msgs::Int64 &msg)
{
count = msg.data;
}

ros::NodeHandle nh;
ros::Subscriber<std_msgs::Int64> sub("counting", callback);

/* UART Communication */
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}

static void MX_USART3_UART_Init(void)
{

/* USER CODE BEGIN USART3_Init 0 */

/* USER CODE END USART3_Init 0 */

/* USER CODE BEGIN USART3_Init 1 */

/* USER CODE END USART3_Init 1 */
huart3.Instance = USART3;
huart3.Init.BaudRate = 115200;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
huart3.Init.Mode = UART_MODE_TX_RX;
huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart3.Init.OverSampling = UART_OVERSAMPLING_16;
huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart3.Init.ClockPrescaler = UART_PRESCALER_DIV1;
huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart3) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_SetTxFifoThreshold(&huart3, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_SetRxFifoThreshold(&huart3, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_DisableFifoMode(&huart3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART3_Init 2 */

/* USER CODE END USART3_Init 2 */

}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart){
if(huart == &huart3){
// set velocity 0 before uart reinitialization
count = 0;

HAL_UART_DeInit(&huart3);
MX_USART3_UART_Init();
nh.getHardware()->init();
}
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
nh.getHardware()->flush();
}

void setup(void)
{
nh.initNode();
nh.subscribe(sub);
}
void loop(void)
{
nh.spinOnce();
}

:::

:::spoiler 提醒：USART_PORT 更改
:::warning
注意 mainpp.cpp 中，**以下函式中皆有 USART_PORT**，記得更改成對應編號：
static void MX_USART3_UART_Init(void)
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
:::

<font color = "yellow">12. 在 Core/inc 中改寫 mainpp.h。並在 main.c 中 include mainpp.h。</font>
:::spoiler 程式碼 -- mainpp.h

#ifndef _MAINPP_H_
#define _MAINPP_H_

#ifdef __cplusplus
extern "C"
{
#endif

void setup(void);
void loop(void);

extern int count;

#ifdef __cplusplus
}
#endif

#endif

:::

:::spoiler 程式碼 -- include mainpp.h

/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mainpp.h"
/* USER CODE END Includes */

:::

$\;$
<font color = "yellow">13. 插上 STM，按 `F11` 進行 Debug 。</font>

現階段完成圖。

![](https://i.imgur.com/IidMLBL.png )

### <font color = "pink">3-3. ROS 和 STM 通訊 -- Python </font>

<font color = "yellow">1. 硬體配置。</font>

![](https://i.imgur.com/giwwHre.png)

( 圖片來源：https://hackmd.io/@JINGCCC/rosserial_1 )

<font color = "yellow">2. 確認接口名稱。</font>

dmesg | grep tty

![](https://i.imgur.com/CAA51QD.png)

如黃色框框所示，我的接口是 ttyUSB0，通常預設值都是這個。

<font color = "yellow">3. 打開接口權限。</font>

sudo chmod 777 /dev/ttyUSB0

<font color = "yellow">4. 進行通訊（記得 roscore）。</font>

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

成功畫面如下：

![](https://i.imgur.com/79zyUMN.png)

:::success
通訊的 node 要留著，如果要 rosrun 其他程式要開新視窗，除非不需要通訊了。
:::

---

:::warning
**CPP 通訊方式有誤，我還在努力 debug。**
上述範例只示範 ROS 當 publisher 而 STM 當 subscriber。
你們如果成功寫出 **STM publish 資料給 ROS subscriber**，歡迎分享你的做法。
要注意，STM 的 publish 頻率約在 100 Hz 上下。請進中斷控制，不然會爆炸。
:::

---
### <font color = "pink">3-4. ROS 和 STM 通訊 -- Cpp </font>

幾乎所有步驟和 Python 一樣，以下是幾個要修改的地方。

<font color = "yellow">1. ROS：rosserial_server package。</font>

在 ROS 的 `${workspace}/src` 資料夾下載 package 然後把 `rosserial_server` 挪進來。 

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

# skip ...

<font color = "yellow">2. ROS：修改 `rosserial_server/include/session.h` 。 </font>

:::spoiler 程式碼

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));

:::


![](https://i.imgur.com/umwVsBN.png)


<font color = "yellow">3. ROS：新增 launch 檔。（自行修改 port 和 topic name 等參數） </font>

<launch>
<node name="serial_server" pkg="rosserial_server" type="serial_node" output="screen" respawn="true" respawn_delay="3">
<param name="port" value="/dev/ttyUSB0" />
<param name="baud" value="115200" />

<rosparam>
require:
publishers: [ ]
subscribers: [ counting ]
</rosparam>
</node>

</launch>

:::success
* **publishers 和 subscribers 的主詞是 STM32。**
* **多個 topic 時命名如下：**
 publishers: [ mecanum_fromSTM, reset_fromSTM, scaraflag_fromSTM ]
* launch 至少要包含 serial_server，也可以再新增自己寫的 node。
這樣就可以**一次開啟通訊和測試程式的 node**。
:::
	
<font color = "yellow">4. 測試通訊。</font>
	**ROS：** launch step3 的 .launch。
	**STM：** F8。