[続]STM32でmicro-ROSを使う

今回はmicro-ROSでカスタムメッセージ、サービス通信、ROS_DOMAIN_IDを実装していく。基本的に[こちらの記事](https://qiita.com/hirekatsu0523/items/83e1503ac4c7fdecd217)を読んでいることを前提として話を進めるので、読んでない人は先に読んでおくと良いかもしれない。
Ubuntu22.04, ROS2 Humble, CubeIDE, F767ZIを用いる。

# カスタムメッセージを作る
　マイコン上でカスタムメッセージを使うための準備段階として、この章ではまず普通にカスタムメッセージを作っていく。

今回はパッケージ名を`custom_test_msgs`とした。
```
// 普段使っているワークスペースに移動する
cd ~/ros2_ws/src
ros2 pkg create custom_test_msgs --build-type ament_cmake --dependencies rosidl_default_generators std_msgs
```
メッセージ型は`AddThreeInts`という名前にする。
```
cd custom_test_msgs
mkdir srv && cd srv
nano AddThreeInts.srv
```
今回は３つの整数を受け取り、その和を返すようなプログラムを作りたいので以下のようにする。
```
int64 a
int64 b
int64 c
---
int64 sum
```
`Ctrl`+`S`で保存、`Ctrl`+`X`で閉じる。

次に`package.xml`を編集する。
```
cd ..
nano package.xml
```
適当な場所（14行目あたり）に以下の二行を追加する。
```xml
  <exec_depend>rosidl_default_runtime</exec_depend>
  <member_of_group>rosidl_interface_packages</member_of_group>
```
保存して閉じる。

次に`CMakeLists.txt`を編集する。
```
nano CMakeLists.txt 
```
プロジェクト名が同じであれば、以下の内容をそのままコピペすれば良い。ament_cmake_autoを使うと簡潔に記述できるのでおすすめだ。
```
cmake_minimum_required(VERSION 3.8)
project(custom_test_msgs)

find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/AddThreeInts.srv"
)
ament_auto_package()
```
これでカスタムメッセージを作成できたので、ビルドする。
```
cd ~/ros2_ws
colcon build
```
エラーが出なければOK。

# カスタムメッセージをマイコンで使えるようにする
次に先ほど作成したカスタムメッセージをマイコン上で使えるようにしていく。

まずはCubeIDEで[前回の記事](https://qiita.com/hirekatsu0523/items/83e1503ac4c7fdecd217)で作成したプロジェクトを開き、`micro_ros_stm32cubemx_utils/microros_static_library_ide/library_generation/extra_packages/`下に先ほど作成したカスタムメッセージをプロジェクトごとコピペする。

今回であれば下の画像のようになる。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/5fe1cb27-3e54-b40d-d178-d25affeb1954.png)

次にコピペ先のフォルダに存在する`extra_packages.repos`を編集し、以下のようにする。
control_msgsの部分は最初から書き込まれている部分なので、それ以降の部分をカスタムメッセージのパッケージ名に応じて追加すること。
```
repositories:
  control_msgs:
    type: git
    url: https://github.com/ros-controls/control_msgs
    version: foxy-devel
    
  custom_test_msgs:
    type: local
    path: extra_packages/custom_test_msgs
```
`micro_ros_stm32cubemx_utils/microros_static_library_ide/libmicroros/libmicroros.a`を削除してからコンパイルすると、ライブラリが再生成される。

`micro_ros_stm32cubemx_utils/microros_static_library_ide/libmicroros/include/`下に`custon_test_msgs`というフォルダが生成されていればOK。

時々docker関連でエラーが出るが、PCを再起動すると大抵直る印象だ。

# カスタムメッセージを利用したサービスサーバの実装
次に先程生成したカスタムメッセージを用いてサービスサーバを実装していく。前回実装したトピック通信のプログラムと比較すると、FREERTOSやmicro-ROSの初期化の部分は変えずに、それ以外の部分をサービスサーバ用に変更している。

`/Core/Src/freertos.c`に以下の内容をコピペする。
```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 "usart.h"
// カスタムメッセージのインクルード
#include <custom_test_msgs/srv/add_three_ints.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);}}

// サービスのリクエストとレスポンスの構造体を定義
custom_test_msgs__srv__AddThreeInts_Request req;
custom_test_msgs__srv__AddThreeInts_Response res;

// FREERTOSのための設定
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) {
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
}

// サービスのコールバック関数を定義
void service_callback(const void *request, void *response)
{
	// リクエストとレスポンスを適切な型にキャスト
	custom_test_msgs__srv__AddThreeInts_Request *_req = (custom_test_msgs__srv__AddThreeInts_Request *)request;
	custom_test_msgs__srv__AddThreeInts_Response *_res = (custom_test_msgs__srv__AddThreeInts_Response *)response;
	// 1秒スリープ（重い処理の代わり）
	osDelay(1000);
	// レスポンスに合計値をセット
	_res->sum = _req->a + _req->b + _req->c;
}

// FREERTOSで実行される部分
void StartDefaultTask(void *argument)
{
    // micro-ROSの初期化
	printf("start default task");
	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__);
	}

    // ここからサービスサーバの実装
	setvbuf(stdout, NULL, _IONBF, BUFSIZ);
	rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
	rcl_allocator_t allocator = rcl_get_default_allocator();
	rclc_support_t support;
	rcl_node_t node = rcl_get_zero_initialized_node();
	rcl_service_t service = rcl_get_zero_initialized_service();
	rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
	rcl_node_options_t node_ops = rcl_node_get_default_options();

	RCCHECK(rcl_init_options_init(&init_options, allocator));
    // ROS_DOMAIN_IDの設定。今回は123としてる。
	RCCHECK(rcl_init_options_set_domain_id(&init_options, 123));
	rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
    // ノードの作成
	RCCHECK(rclc_node_init_with_options(&node, "simple_server", "", &support, &node_ops));
    // サービスの作成
	RCCHECK(rclc_service_init_default(&service, &node, ROSIDL_GET_SRV_TYPE_SUPPORT(custom_test_msgs, srv, AddThreeInts), "add_three_ints"));
    // エグゼキューターの作成。三番目の引数はextecuterに登録するコールバック関数の数。
	RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
    // エグゼキューターにサービスを追加
	RCCHECK(rclc_executor_add_service(&executor, &service, &req, &res, &service_callback));

	while(1){
		// エグゼキューターを実行してリクエストを処理
		rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
	    osDelay(10);
	}
	// 終了処理
	RCCHECK(rcl_service_fini(&service, &node));
	RCCHECK(rcl_node_fini(&node));
}
```
コンパイルがうまく行けばOK。ちなみに2023年9月現在においてmicro-ROSで二つ以上のサービスサーバーを扱うことはできないと思われるので注意する事。

# 使ってみる
ROS_DOMAIN_IDを設定していない人は`~/.bashrc`に設定しておくこと。
```
export ROS_DOMAIN_ID=123
```

以下のコマンドを打ってmicro-ros-agentを起動する。
```
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
```
マイコンとPCをUSBで接続し、ボード上の黒いボタンを押せばAgentがマイコンを認識する。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/2ca88122-c419-fd90-4c13-7807bb827c29.png)

rqtの`Plugins`>`Services`>`Service Caller`を開くと、マイコン上のサービスサーバーが認識されていることが分かる。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/681243e2-5c07-51c6-2578-0a170ce3c2de.png)

うまく実装できていれば、a, b, cに適当な値を入力してサービスをCallするとその和が返ってくる。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/d89faca7-2fbc-beeb-7da9-ebe5e9fcdb06.png)

# おまけ
一応PC側のクライアントも実装したので供養。
```cpp
#include "custom_test_msgs/srv/add_three_ints.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
#include <cinttypes>
#include <iostream>
#include <string>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;
using namespace custom_test_msgs::srv;

class SimpleClient : public rclcpp::Node{
  public:
  SimpleClient() : Node("simple_client"){
    client_ = create_client<AddThreeInts>("add_three_ints");
    while (!client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        return;
      }
      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available");
    }
    auto request = std::make_shared<AddThreeInts::Request>();
    request->a = 1;
    request->b = 12;
    request->c = 3;
    {
      using ServiceResponceFuture = rclcpp::Client<AddThreeInts>::SharedFuture;
      auto response_received_callback = [this](ServiceResponceFuture future) {
        auto response = future.get();
        RCLCPP_INFO(this->get_logger(), "%ld", response->sum);
        rclcpp::shutdown();
      };
      auto future_result = client_->async_send_request(request, response_received_callback);
    }
  }
  private:
  rclcpp::Client<AddThreeInts>::SharedPtr client_;
};

int main(int argc, char **argv)
{
  std::cout << "hello from client";
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SimpleClient>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
```