Go to Qiita Advent Calendar 2024 Top
LoginSignup
9
4

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.

ROS2Advent Calendar 2022

Day 4
@hbvcg00(Tatsuki Nishimura)

mros2-mbed使ってみた

Last updated at Posted at 2022-12-04

概要

ROS2とマイコン通信の新たなデファクトスタンダードになる（と勝手に期待している）mROS2-mbedを使ってみた記事です。

mROS2-mbedとは？

ROS2とEthernetがついたマイコンとの通信のライブラリです。ROS-マイコン通信といえばrosserialやmicro-ROSなどがありますが、このmROS2-mbedが優れている点としては、

  • Ethernetで通信ができる
  • mbed標準の環境を汚すことなく導入できる
  • 新しい

ことだと思っています。
特に2番目についてですが、rosserialやgcc4mbedなどのROSとの通信ライブラリはmbedの標準ビルドツールである
mbed-cliが使えませんでした。一方、mROS2-mbedはmbed-cliの後継であるmbed-cli2に対応しており、mbed OSのバージョンも最新の6で作られています。

使ってみる

今回はマイコン側ではDockerを用いず、ネイティブ環境でセットアップします。

環境

PC

  • Ubuntu 20.04
  • ROS2 foxy（docker）
  • CMake 3.19以上
    • Ubuntu 20.04ではCMakeのバージョンが古いので、CMakeのREADMEを参考に3.19以上のバージョンをビルドしてください。

※mros2-mbedは公式ではfoxyかhumbleにのみ対応しています。galacticで使う場合は以下を参照してください。

マイコン

mbed-CLI2の環境構築（Ubuntu 20.04）

arm-none-eabi-g++ のインストール

Mbed環境がない場合は、mbed-cli2に加えてARMのツールチェインであるarm-none-eabi-g++をインストールする必要があります。
バージョンは2021.07を使用します。(aptで最新バージョンを入手しないでください)

まずはGNU Arm Embedded Toolchain Downloadsから gcc-arm-none-eabi-10.3-2021.07-x86_64-linux.tar.bz2 (Ubuntu20.04, x86-64CPUの場合) をダウンロードします。

ファイルgcc-arm-none-eabi-10.3-2021.07-x86_64-linux.tar.bz2が~/Downloadsにダウンロードされた後は、次のコマンドを実行してインストールを行います。

sudo mkdir /opt/cmake
sudo tar -jxvf ~/Downloads/gcc-arm-none-eabi-10.3-2021.07-x86_64-linux.tar.bz2 -C /opt/cmake
export PATH=$PATH:/opt/cmake/gcc-arm-none-eabi-10.3-2021.07/bin/

ツールチェインのPATH(環境変数)ついて、毎回立ち上げ直すたびにリセットされるため予め登録する方法があります。
任意のエディタで ~/.bashrc を開いて末尾に export PATH=$PATH:/opt/cmake/gcc-arm-none-eabi-10.3-2021.07/bin/ を追記して保存します。


1. マイコンとPC、もしくはマイコンとルーターをLANケーブルで接続しておいてください。

僕はルーターとマイコンを接続しました。

2. ifconfigコマンドなどで自分のIPアドレスを確認します。

僕の環境では192.168.0.101でした。

3. 適当な場所に

```
git clone https://github.com/mROS-base/mros2-mbed.git
```

で落としてきます。

4.workspaceフォルダ内にいくつか例があるので、echoback_stringを実行することにします。

workspace/echoback_string/app.cppを編集します。

app.cpp(一部抜粋)
#include "mbed.h"
#include "mros2.h"
#include "std_msgs/msg/string.hpp"
#include "EthernetInterface.h"

#define IP_ADDRESS ("192.168.11.2") /* IP address */
#define SUBNET_MASK ("255.255.255.0") /* Subnet mask */
#define DEFAULT_GATEWAY ("192.168.11.1") /* Default gateway */

IPアドレスとデフォルトゲートウェイの右から2番目の11を自分のIPアドレスと同じ数字に変更します。
例えば、僕のIPアドレスは192.168.0.101なので、それぞれ192.168.0.2と192.168.0.1に変更します。

注意

事前にIPアドレスがかぶってないか確認しておいてください。
arp -a でネットワーク内の機器一覧を確認できます。すでに192.168.0.2が存在する場合、192.168.0.3 などかぶらないIPアドレスを設定します。

同様にinclude/rtps/config.hも編集します。

config.h(一部抜粋)
namespace Config {
const VendorId_t VENDOR_ID = {13, 37};
const std::array<uint8_t, 4> IP_ADDRESS = {
    192, 168, 11, 2}; // Needs to be set in lwipcfg.h too.
const GuidPrefix_t BASE_GUID_PREFIX{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12};

IP_ADDRESS配列の11を自分のIPのものに変更します。

5.ビルドする

./build.bash all NUCLEO_F767ZI echoback_string native

最初のビルドだと大体30秒から1分はかかります。

6.ROS2と通信

https://github.com/mROS-base/mros2-host-examples
から落としてビルドするかdockerを使うなどしてサンプルを実行します。今回はdockerを使いました。

リポジトリより引用
docker run --rm -it --net=host ros:humble /bin/bash \
  -c "source /opt/ros/humble/setup.bash &&
  cd &&
  git clone https://github.com/mROS-base/mros2-host-examples &&
  cd mros2-host-examples &&
  colcon build --packages-select mros2_echoreply_string &&
  source install/setup.bash &&
  ros2 run mros2_echoreply_string echoreply_node"

7.シリアル通信で結果を確認する

適当なツールやコマンドでシリアルの内容を確認します。

publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 22'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 23'
[MROS2LIB] subscriber matched with remote publisher
[MROS2LIB] publisher matched with remote subscriber
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 24'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 24'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 25'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 25'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 26'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 26'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 27'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 27'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 28'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 28'
publishing msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 29'
subscribed msg: 'Hello from mros2-mbed onto NUCLEO_F767ZI: 29'

ちゃんと通信できていることがわかります。

メッセージを自作してみる

mROS2-mbedではメッセージ定義を自作できます。最初から用意されているメッセージ型はstd_msgsと一部のgeometry_msgsだけなので、この機能を使ってROS2のメッセージを一つ追加してみようと思います。

今回はtfを出力させるため、tf2_msgs/msg/TFMessageを定義します。
まず、TFMessageの定義は次のようになっています。
https://docs.ros2.org/foxy/api/tf2_msgs/msg/TFMessage.html

TFMessage
geometry_msgs/msg/TransformStamped[] transforms

要はgeometry_msgs/msg/TransformStampedを配列にしただけです。しかし、geometry_msgs/msg/TransformStampedは用意されていないので、これも自作します。
https://docs.ros2.org/latest/api/geometry_msgs/msg/TransformStamped.html
geometry_msgs/msg/Transformも用意されていないので、自作します。
https://docs.ros2.org/latest/api/geometry_msgs/msg/Transform.html

運良くVector3とQuaternionは用意されていたので、Transformから作り始めます。

  1. msgファイルを作る
    workspace/custom_msgs/geometry_msgs/msgにTranform.msgを作成します。
Transform.msg
geometry_msgs/msg/Vector3 translation
geometry_msgs/msg/Quaternion rotation

2.cd workspaceでworkspaceディレクトリに移動した後、

python3 ../mros2/mros2_header_generator/header_generator.py geometry_msgs/msg/Transform.msg

と入力します。すると、workspace/custom_msgs/geometry_msgs/msgにTransform.hppが生成されます。
header_generator.pyでは、主にシリアライズ、デシリアライズの関数を生成します。copyToBuf()がシリアライズ、copyFromBuf()がデシリアライズです。これらの関数によって、データ←→バイト列の変換を行い、ROSのTopicとして扱えるようになります。

3.同様にしてTransformStamped,TFMessageに対しても1. 2.を行います。
注意点としては、
TranformStamped.msgにおいてheaderの定義はstd_msgs/msg/Headerと書くのではなく、headerと書くことです。このように書かないと認識してもらえません。
また、TFMessageはgeometry_msgsではなくtf2_msgsですから、新しくディレクトリを作成して、そちらに作ったほうがいいと思います。

TranformStamped.msg
header header
string child_frame_id
geometry_msgs/msg/Transform transform

4.微修正
https://github.com/TatsukiNishimura/mros2-mbed/commit/9a2688c8d02c2541d0733ce8029368732fce164d#diff-1228b1d2eff580e8c82466fbcf0a820a774750b4dc65138d8a6c5095b4b5d50f
このコミットにしたがってTransformStamped.hppとTFMessage.hppを修正します。（見にくくてすみません）
具体的にはTransformstamped.hpp内にcopyToBuf()のオーバーロードを作り、
https://github.com/TatsukiNishimura/mros2-mbed/commit/9a2688c8d02c2541d0733ce8029368732fce164d#diff-1228b1d2eff580e8c82466fbcf0a820a774750b4dc65138d8a6c5095b4b5d50fR94-R149
それをTFMessage.hppのhttps://github.com/TatsukiNishimura/mros2-mbed/commit/9a2688c8d02c2541d0733ce8029368732fce164d#diff-b1caa1ac2a24a3b55132ff6f55e7f3a645186486da4b56d52be24267fbd4d5d4R45
で使用します。こうしないとROS2側のdeserializeでエラーを吐きます。ここで実は数時間溶かしています…

また、mros2.cppのbuf[100]もbug[255]くらいに変更してください。

TFを出したかった

上で作ったメッセージ定義を使って、TFを出してみようと思います。

  1. workspaceにpub_tfというフォルダを作り、中にapp.cppとtempates.hppを用意します。中身は空でいいです。
    ちなみにファイル名はこれ以外は使えないです。別の名前を使いたい場合は、CMakeLists.txtとmros2/mros2_header_generator/templates_generator.pyを適当に編集すればいけます。

2.app.cppを編集する
https://github.com/TatsukiNishimura/mros2-mbed/commit/9a2688c8d02c2541d0733ce8029368732fce164d#diff-870c785a151673f2f6e50f43a3aa6298edb5033be0f45bd15481d4f7038d255c

app.cpp(一部抜粋)
int main()
{
  EthernetInterface network;
  network.set_dhcp(false);
  network.set_network(IP_ADDRESS, SUBNET_MASK, DEFAULT_GATEWAY);
  nsapi_size_or_error_t result = network.connect();

  // 現在時刻を取得
  SocketAddress sockAddr;
  network.gethostbyname("time.nist.gov", &sockAddr);
  sockAddr.set_port(37);
  ntp_packet in_data;
  UDPSocket sock;
  sock.open(&network);
  char out_buffer[] = "time";
  if (0 > sock.sendto(sockAddr, out_buffer, sizeof(out_buffer)))
  {
    printf("Error sending data\n");
    return -1;
  }
  if (sock.recvfrom(&sockAddr, &in_data, sizeof(ntp_packet)) > 0)
  {
    in_data.secs = ntohl(in_data.secs) - 2208988800; // 1900-1970
  }
  sock.close();

  Timer stm_clock;
  stm_clock.reset();
  stm_clock.start();

  printf("mbed mros2 start!\r\n");
  printf("app name: pub_tf\r\n");
  mros2::init(0, NULL);
  MROS2_DEBUG("mROS 2 initialization is completed\r\n");

  mros2::Node node = mros2::Node::create_node("mros2_node");
  mros2::Publisher pub = node.create_publisher<tf2_msgs::msg::TFMessage>("tf", 10);
  osDelay(100);
  MROS2_INFO("ready to pub/sub message\r\n");
  int publish_count = 0;

  while (1)
  {
    // 必ずループごとにインスタンスを生成する
    // そうじゃないとエラーが起こる（理由不明、copyfrombufあたりがミスってる）
    tf2_msgs::msg::TFMessage tf;
    std::vector<geometry_msgs::msg::TransformStamped> trans_vec;
    trans_vec.resize(1);
    geometry_msgs::msg::TransformStamped transformstamped;
    geometry_msgs::msg::Transform transform;
    geometry_msgs::msg::Quaternion quat;
    geometry_msgs::msg::Vector3 vec;
    transformstamped.frame_id = "odom";
    transformstamped.child_frame_id = "base_link";
    vec.x = static_cast<double>(publish_count * 0.00001);
    vec.y = -static_cast<double>(publish_count * 0.00001);
    vec.z = 0.0;
    quat.x = 0.0;
    quat.y = 0.0;
    quat.z = static_cast<double>(publish_count * 0.00001);
    quat.w = static_cast<double>(publish_count * 0.00001);
    transform.rotation = quat;
    transform.translation = vec;
    transformstamped.transform = transform;
    const int usec = stm_clock.read_us();
    const int sec = static_cast<int>(usec * 0.000001);
    transformstamped.sec = in_data.secs + sec;
    transformstamped.nanosec = (usec - (sec * 1000000)) * 100;

    trans_vec.at(0) = transformstamped;
    tf.transforms = trans_vec;
    pub.publish(tf);
    osDelay(100);
    publish_count++;
  }

このように書きました。
最初にtime.nist.govにアクセスして現在時刻を取得します。ROSとの時刻合わせ用です。
whileループでは1ループごとにTFの値を変化させてpublishしています。
なお、未検証ですがメッセージの変数はループ内で宣言したほうが良さそうです。ループの外側で宣言するとエラーで落ちるときがあります。

3.確認
ros2 topic echoなどでtfの値を確認します。

transforms:
- header:
    stamp:
      sec: 1670159046
      nanosec: 70907000
    frame_id: odom
  child_frame_id: base_link
  transform:
    translation:
      x: 0.09586000000000001
      y: -0.09586000000000001
      z: 0.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.09586000000000001
      w: 0.09586000000000001

ちゃんと出ていますね。じゃあrvizやview_framesでも確認するかーと思っていたんですが、なんとtfが認識されないです。
多分ですが、 ただ/tfトピックをSubscribeするのとtf_listenerを使ってlistenするのでは微妙に違うんじゃないかと思いました。時間不足で調べられていません。すみません。強い人が解決してくれることを願っています。

あとがき

結局うまいこと紹介できないまま終わってしまった。本当は実機に載せて制御したり、マイコン-マイコン通信も試してみたかったんですが、アドカレに間に合わせるために省いてしまいました。
mros2-mbedはうまく使えば絶大な威力を発揮すると思っています。これだけ手軽にマイコンとROSで通信ができるのは驚きでした。いずれ学ロボでROS2がデフォルトになった時は、このライブラリが広く使われることになると思います。皆で使ってどんどんPR出しましょう！

9
4
4

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
9
4

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?