LoginSignup
2
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@hirekatsu0523

[続]STM32でmicro-ROSを使う

Last updated at Posted at 2023-08-20

今回はmicro-ROSでカスタムメッセージ、サービス通信、ROS_DOMAIN_IDを実装していく。基本的にこちらの記事を読んでいることを前提として話を進めるので、読んでない人は先に読んでおくと良いかもしれない。
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行目あたり）に以下の二行を追加する。

  <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で前回の記事で作成したプロジェクトを開き、micro_ros_stm32cubemx_utils/microros_static_library_ide/library_generation/extra_packages/下に先ほど作成したカスタムメッセージをプロジェクトごとコピペする。

今回であれば下の画像のようになる。
image.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に以下の内容をコピペする。

#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

rqtのPlugins>Services>Service Callerを開くと、マイコン上のサービスサーバーが認識されていることが分かる。
image.png

うまく実装できていれば、a, b, cに適当な値を入力してサービスをCallするとその和が返ってくる。
image.png

おまけ

一応PC側のクライアントも実装したので供養。

#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;
}
2
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?