STM32でmicro-ROSを使う

本記事はUbuntu22.04, ROS2-humbleを前提とする。使用するマイコンはstm32f767ziだが，その他のSTM32マイコンでもある程度のスペックがあれば似たような手順で使用できると思われる。

# micro-rosとは
https://micro.ros.org/

micro-ROSはROS2をマイクロコントローラで使用するためのオープンソースプロジェクトである。ROS2ノードをマイクロコントローラ上に実装し、共通のフレームワークを使用することで、PCとマイコンをつなぐことができる。
開発は現在においても活発で，ドキュメントも充実しつつあるため，micro-ROSはマイコンとROS2をつなぐ場合のスタンダードとなりつつあると言っても良い。とはいえ日本語での実装記事は未だに少ないのが現状なので，この記事が一つの参考になればと思う。


# micro-ROS-Agentのインストール

まずmicro-ROSのビルドシステムをインストールしていく。
```
source /opt/ros/humble/setup.bash
mkdir uros_ws && cd uros_ws
git clone -b humble https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup
rosdep update && rosdep install --from-paths src --ignore-src -y
colcon build
source install/local_setup.bash
```
次にmicro-ROS-Agentをビルドする。
```
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash
```
以下のコマンドを打ち込んでいい感じにAgentが起動すればうまく行っている。
```
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
```

# CubeIDEのインストール
[こちらのリンク](https://www.st.com/ja/development-tools/stm32cubeide.html#st-get-software)からSTの公式ページに飛んで`STM32CubeIDE-DEB`をダウンロードする。その際にログイン，または登録を求められるので適宜対応すること。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/bb21b64f-8a40-43d6-523c-92f7cea2f4b9.png)

`Ctrl`+`Alt`+`T`でターミナルを開いたらダウンロードフォルダまで移動してダウンロードファイルを解凍する。
```
cd Downloads
unzip en.st-stm32cubeide_1.13.0_17399_20230707_0829_amd64.deb_bundle.sh.zip
```
解凍して出てきた.shファイルを実行する。
```
sudo bash st-stm32cubeide_1.13.0_17399_20230707_0829_amd64.deb_bundle.sh 
```
するととてつもなく長い英文が表示されるのでスペースキーを連打して読み飛ばす。途中で選択肢が出てくるのですべて`y`と入力して進める。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/6daefed1-bb7b-7fc8-ecc8-12950c994d5b.png)

インストールが終わると検索からソフトが表示されるようになるので起動する。
画面上部の`My ST`と書かれた部分をクリックして先程登録したアカウント情報を入力すること。

# CubeIDEでプロジェクトの作成
CubeIDEを開いて，左上の`File`>`New`>`STM32 Project`から新しくF767ZIのプロジェクトを作成する。
左上の検索窓にF767ZIなどと打ち込めばSTM32F767ZIT6ボードが表示されるので，選択し右下の`Next>`をクリックする。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/37e25029-32d5-f958-3ce2-542f76ede080.png)

今回はプロジェクト名を`uros_test`とするが，これは別に何でも良い。

### Pinout & Configuration

設定画面が開いたらPinout & ConfigurationからPinの設定をしていく。
 - System Core > RCC
HSE:Crystal/Ceramic Resonator(あくまでも一例)

 - System Core > SYS
Timebase Source:TIM1(なんでもいい)

 - Connectivity > USART3
Mode:Asynchronous

    DMA Settings > Add > USART3_RX
DMA Request Setting > Mode:Circular
Priority:Very High

    DMA Settings > Add > USART3_TX
Priority:Very High

    NVIC Settings
USART3 global interrupt:Enable

USART3_RXとUSART3_TXのピンをPD9,PD8に変更する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/3d66d2d8-5ce8-87ac-b6d3-aa6e0152131c.png)

 - Connectivity > USB_OTG_FS
External Phy:Device_Only

 - Middleware and Software Packs > FREERTOS
Interface:CMSIS_V2
Configuration > Tasks and Queues > Tasks > defaultTask
ダブルクリックして設定画面を開いたら以下のように設定する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/4e19a68d-e54f-5086-6117-d7a8df79b985.png)

Stack Sizeの3000という値は最低限の値なので，増やせるならば増やした方が良いと思われる。

 - Middleware and Software Packs > USB_DEVICE
Class For FS IP:Communication Device Class(Virtual Port Com)


### Clock Configuration

次にClock Configurationを開き次のように変更する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/36ce4234-4bb3-ca16-b7d9-6baa7d30f045.png)

※これはあくまでも一例であり，それ以外では動作しないという訳ではない。


### Project Manager

最後にProject Managerを開いて次のように設定していく。
 - Code Generator
`Generate peripheral initialization as a pair of .c/.h files per peripheral`を有効化する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/944b5b92-10e5-5108-1c5a-fce24ff89df2.png)


以上の操作が終わったらスタンプマークからgenerate codeする。

# micro_ros_stm32cubemx_utilsの導入
`Ctrl`+`Alt`+`T`でターミナルを開き，先ほど作成したプロジェクトのフォルダまで移動してgithubリポジトリをダウンロードする。
```
// 作成したプロジェクトのフォルダまで移動する
cd STM32CubeIDE/workspace_1.13.0/uros_test/
git clone -b humble https://github.com/micro-ROS/micro_ros_stm32cubemx_utils.git
```
CubeIDEに戻り，Project Explorer上のプロジェクト名を右クリックして，`Refresh`を選択する。するとダウンロードした`micro_ros_stm32cubemx_utils`がProject Explorerに表示されるようになる。

次にプロジェクト名を右クリックして`Properties`>`C/C++ Build`>`Settings`>`Build Steps`>`Pre-build steps`を開く。`Command:`の部分に以下のコマンドを入力する。
```
sudo docker pull microros/micro_ros_static_library_builder:humble && sudo docker run --rm -v ${workspace_loc:/${ProjName}}:/project --env MICROROS_LIBRARY_FOLDER=micro_ros_stm32cubemx_utils/microros_static_library_ide microros/micro_ros_static_library_builder:humble
```

次にプロジェクト名を右クリックして，`Properties`>`C/C++ Build`>`Settings`>`Tool Settings`>`MCU GCC Compiler`>`Include paths`を開く。`Include paths(-l)`の右側の紙に＋が付いたマークをクリックして，
`../micro_ros_stm32cubemx_utils/microros_static_library_ide/libmicroros/include`
と入力する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/bfafa022-86ec-8e64-73eb-51339ad5a4c9.png)

同じく，`C/C++ Build`>`Settings`>`Tool Settings`>`MCU GCC Linker`>`Libraries`と移動して，`Library search path (-L)`に
`<絶対パス>/micro_ros_stm32cubemx_utils/microros_static_library_ide/libmicroros`
を追加する。`<絶対パス>`には`micro_ros_stm32cubemx_utils`までの絶対パスを入力すること。例えば自分の場合だと
`/home/ユーザー名/STM32CubeIDE/workspace_1.13.0/uros_test/micro_ros_stm32cubemx_utils/microros_static_library_ide/libmicroros`
となった。
同様にして同じページの`Libraries (-l)`に`microros`と追加する。

次に以下のファイルをプロジェクトの`/Core/Src`下に移動させる。
 - micro_ros_stm32cubemx_utils/extra_sources/microros_time.c
 - micro_ros_stm32cubemx_utils/extra_sources/microros_allocators.c
 - micro_ros_stm32cubemx_utils/extra_sources/custom_memory_manager.c
 - micro_ros_stm32cubemx_utils/extra_sources/microros_transports/dma_transport.c


このままコンパイルするとsudoでパスワードの入力を求められ，エラーが発生してしまうので，最後にsudoでパスワードを入力しなくても良いように設定する。
ターミナルを開いて以下のコマンドを実行する。
```
sudo visudo
```
該当する部分に以下のような文を追加する。なお`ユーザー名`の部分は自分のユーザー名とすること。
```
# Allow members of group sudo to execute any command
%sudo   ALL=(ALL:ALL) ALL
ユーザー名 ALL=(ALL) NOPASSWD:ALL # 追加
```

# コードの編集
試験的にStringをPublishするノードを実装する。
`/Core/Src/freertos.c`に以下のコードを貼り付ける。
<details><summary>freertos.c</summary><div>

```c
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

#include <stdbool.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <uxr/client/transport.h>
#include <rmw_microxrcedds_c/config.h>
#include <rmw_microros/rmw_microros.h>
#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/string.h>
#include <rosidl_runtime_c/string_functions.h>
#include "usart.h"

typedef StaticTask_t osStaticThreadDef_t;

//エラーハンドリング用のマクロ
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc);}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}

//micro-ROS関連の変数の初期化
rcl_publisher_t publisher;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;
std_msgs__msg__String msg;

osThreadId_t defaultTaskHandle;
uint32_t defaultTaskBuffer[ 3000 ];
osStaticThreadDef_t defaultTaskControlBlock;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .cb_mem = &defaultTaskControlBlock,
  .cb_size = sizeof(defaultTaskControlBlock),
  .stack_mem = &defaultTaskBuffer[0],
  .stack_size = sizeof(defaultTaskBuffer),
  .priority = (osPriority_t) osPriorityNormal,
};

bool cubemx_transport_open(struct uxrCustomTransport * transport);
bool cubemx_transport_close(struct uxrCustomTransport * transport);
size_t cubemx_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);

void * microros_allocate(size_t size, void * state);
void microros_deallocate(void * pointer, void * state);
void * microros_reallocate(void * pointer, size_t size, void * state);
void * microros_zero_allocate(size_t number_of_elements, size_t size_of_element, void * state);

void StartDefaultTask(void *argument);

void MX_FREERTOS_Init(void);

void MX_FREERTOS_Init(void) {
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
}

void StartDefaultTask(void *argument)
{
  // micro-ROSの設定。ここはいじらなくてよい。
  rmw_uros_set_custom_transport(
    true,
	(void *) &huart3,
	cubemx_transport_open,
	cubemx_transport_close,
	cubemx_transport_write,
	cubemx_transport_read
  );

  rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
  freeRTOS_allocator.allocate = microros_allocate;
  freeRTOS_allocator.deallocate = microros_deallocate;
  freeRTOS_allocator.reallocate = microros_reallocate;
  freeRTOS_allocator.zero_allocate =  microros_zero_allocate;

  if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
    printf("Error on default allocators (line %d)\n", __LINE__);
  }

  // ここからmicro-ROSのセットアップ
  allocator = rcl_get_default_allocator();
  node = rcl_get_zero_initialized_node();
  //初期化設定の作成
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  //ノードの作成
  RCCHECK(rclc_node_init_default(&node, "f7_node", "", &support));
  //publisherの作成
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
    "/from_f767zi"));

  //配列データを扱うときの処理
  rosidl_runtime_c__String__init(&msg);
  char hello[] = "Hello world from f7";
  rosidl_runtime_c__String__assignn(&msg.data, hello, sizeof(hello));
  for(;;){
    //データのpublish
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
	osDelay(500);
  }
}
```


</div></details>

コンパイルしてうまく行ったらOK。エラーが出る場合は一度再起動すると良いかも。

# テスト
マイコンとPCをつなぐUSBコードを一度抜き差ししてから，ターミナルを開いて以下のコマンドを実行する。
```
dmesg
```
一例としてこのような行が見つかると思われる。
```
[17779.316475] cdc_acm 1-2:1.2: ttyACM0: USB ACM device
```
この場合，/dev/ttyACM0にマイコンがシリアル接続されていることを意味している。
次いで以下のように打つ。`--dev`のあとにはそれぞれ適当な値を入れること。
```
ros2 run micro_ros_agent micro_ros_agent serial -b 115200 --dev /dev/ttyACM0
```
続いてマイコンに作成したプログラムを書き込む，もしくはすでに書き込んである場合はマイコン上の黒いボタンを押す。
うまく行っていればmicro-ROS-Agentがマイコンを認識し，Publishされたトピックを認識できるようになる。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/9256b456-0eb2-80a7-0bc8-6c63326d536f.png)

# おわり
この記事を参考に実装したという人が居たら嬉しい。途中で詰まった場合は，英語ではあるものの公式ドキュメントがかなり整っているのでそちらを参考にすると良いと思う。

続き↓↓
https://qiita.com/hirekatsu0523/items/492f52c9d697dac1eb29

# 参考文献
https://youtu.be/xbWaHARjSmk
https://qiita.com/kyamawaki/items/bd63614fef8f0544929f
https://github.com/micro-ROS/micro_ros_setup
https://github.com/micro-ROS/micro_ros_stm32cubemx_utils
https://micro.ros.org/docs/tutorials/core/first_application_rtos/freertos/