rclrustを使って1から自分でROS2 ノードを作成してみる

## これは何？

RustでROS2のノードを書く方法についての解説です。基本的には[このドキュメント](https://github.com/ros2-rust/ros2_rust/blob/087527ef12a353c5b4023b242c31c715f5369a14/docs/writing-your-first-rclrs-node.md)に沿ってノードを作成しその動作確認についてまとめていきます。

今回はDockerを使わず、関連ソフトのインストールも手動でやっていきます。

[かなり前の記事](https://qiita.com/MrBearing/items/7e1bd4d1814d3a3b00e4)でros2-rustのについて書いたのですが、中々フォローできず2年も立ってしまいました。。

## 対象とする読者

- RustでROS2を書いてみたいと、常々思っている人
- 一通りRustの基本的な文法標準ライブラリを理解している人

## 環境

- Ubuntu 22.04　(ROS Humble)
- 言語 Rust 1.65.0(1.63以上のインストールが必須)

## 作業準備

### 関連ソフトのインストール
ROS2で使用するの基本的なツール類とcargo及びcolconのプラグインをインストールします。
端末で以下のコマンドを実行してください。

```bash
# 基本的なツール類
sudo apt install -y git libclang-dev python3-pip python3-vcstool
# cargoとcolconへそれぞれお互い用のプラグインをインストール
cargo install --debug cargo-ament-build
pip install git+https://github.com/colcon/colcon-cargo.git
pip install git+https://github.com/colcon/colcon-ros-cargo.git
```

### ワークスペースの作成と関連ソースのインポート
ワークスペースを作成し、ros2_rustのソースコードと関連するパッケージ類をインポートします。
端末で以下のコマンドを実行してください。

```
mkdir -p workspace/src && cd workspace
git clone https://github.com/ros2-rust/ros2_rust.git src/ros2_rust
vcs import src < src/ros2_rust/ros2_rust_humble.repos
```


## 作業内容

### パッケージ作成

パッケージの作成には端末で以下のコマンドをworkspaceディレクトリで実行してください。
cargoを用いてパッケージ作成をします。残念ながら```ros2 pkg create```のようなツールがrclrsにはまだないそうです。

```bash
cargo new republisher_node && cd republisher_node
```

またROS2のビルドに関連するファイルも手動で作成する必要があります。作成したパッケージ直下にpackage.xmlを作成してください。
```xml
<package format="3">
  <name>republisher_node</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="user@todo.todo">user</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclrs</depend>
  <depend>std_msgs</depend>

  <export>
    <build_type>ament_cargo</build_type>
  </export>
</package>
```

CMakeLists.txtは必要ありません。

### コードを記述する

今回は```/in_topic```というトピックを受信したら、```/out_topic```というトピックへ1秒毎にpublishし続けるノードを作成します。

ソースコードは以下の通りです。main.rsを以下の内容で更新してください。

```rust:main.rs
use std::sync::{Arc,Mutex};
use std_msgs::msg::String as StringMsg;

struct RepublisherNode {
    node: rclrs::Node,
    _subscription: Arc<rclrs::Subscription<StringMsg>>,
    publisher: rclrs::Publisher<StringMsg>,
    data: Arc<Mutex<Option<StringMsg>>>,
}

impl RepublisherNode {
    fn new(context: &rclrs::Context) -> Result<Self, rclrs::RclrsError> {
        let mut node = rclrs::Node::new(context, "republisher")?;
        let data = Arc::new(Mutex::new(None));
        let data_cb = Arc::clone(&data);
        let _subscription = node.create_subscription(
            "in_topic",
            rclrs::QOS_PROFILE_DEFAULT,
            move |msg: StringMsg| { 
                *data_cb.lock().unwrap() = Some(msg);
            },
        )?;

        let publisher = node.create_publisher("out_topic",rclrs::QOS_PROFILE_DEFAULT)?;

        Ok(Self {
            node,
            _subscription,
            publisher,
            data,
        })
    }

    fn republish(&self) -> Result<(),rclrs::RclrsError>{
        if let Some(s) = &*self.data.lock().unwrap(){
            self.publisher.publish(s)?;
        }
        Ok(())
    }
}

fn main() -> Result<(), rclrs::RclrsError> {
    let context = rclrs::Context::new(std::env::args())?;
    let republisher = Arc::new(RepublisherNode::new(&context)?);
    let republisher_other_thread = Arc::clone(&republisher);　
    std::thread::spawn(move || ->Result<(),rclrs::RclrsError> {
        loop {
            use std::time::Duration;
            std::thread::sleep(Duration::from_millis(1000));
            republisher_other_thread.republish()?;
        }
    });
    rclrs::spin(&republisher.node)
}


```

個人的にはかなり素直なRustで書けている印象です。
Arc\<Mutex\<XXX\>\>等の多重ジェネリクスがあるので、若干びっくりしますが、Rustのマルチスレッドの基本的な書き方で書かれています。

dataで受け取ったメッセージをpublisherが一秒毎に、トピックとして送信しています。

### 実行方法1(cargoを使う方法)

作成したパッケージのディレクトリ下で端末を開き```cargo run```を実行します。
別の端末(端末A)を開き以下のコマンドを実行します。
```bash
ros2 topic echo /out_topic
```
さらに別の端末(端末B)を開き、以下を実行します。
```bash
ros2 topic pub /in_topic std_msgs/msg/String '{data: "Bonjour"}' -1
```

端末Aで以下が表示されれば成功です。

```bash
$ ros2 topic echo /out_topic
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
```

### 動作確認2(colconを使う方法)
ワークスペース直下に移動し、以下のコマンドを用いてビルドします
```bash
colcon build
```
実行時は以下のコマンドを使ってください。(端末A)

```bash
ros2 run republisher_node republisher_node
```

別の端末(端末B)を開き以下のコマンドを実行します。
```bash
ros2 topic echo /out_topic
```

さらに別の端末(端末C)を開き、以下を実行します。
```bash
ros2 topic pub /in_topic std_msgs/msg/String '{data: "Bonjour"}' -1
```

端末Bで以下が表示されれば成功です。

```bash
$ ros2 topic echo /out_topic
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
data: Bonjour
---
```

## 成果物

Githubに作ったものをまとめておいておきましたので、参考にしてください

https://github.com/MrBearing/republisher_node

## 私が詰まったところ
- std_msgs等のメッセージはソースから持ってこないとビルドできない
- rustcのバージョンが低いのでrustupを使ってバージョンを上げる必要があった(1.63以上)


## 参考資料
- https://github.com/ros2-rust/ros2_rust/blob/main/docs/writing-your-first-rclrs-node.md
- https://github.com/ros2-rust/ros2_rust/blob/main/docs/building.md