r2rを使用して自分で作ったノードをcolconでビルドする

## これは何？
r2rを使って自分で作ったノードをcolconでビルドする方法のまとめです。

@OTLさんが[この投稿](https://qiita.com/OTL/items/4112b400cacb653e2d9f)でr2rをオススメしてたので(現状実用できるのはr2rだけっぽい)、colconでビルドする方法をまとめてみました。

## 対象とする読者

- RustでROS2を書いてみたいと、常々思っている人
- 一通りRustの基本的な文法、標準ライブラリを理解している人
- Rustでのマルチスレッドや非同期プログラミングの方法を理解している人

## 環境

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

## 作業準備

特にインストールするものは必要ないです。ROSとRustのインストールは必要なので各自インストールしてください。

## 作業内容
r2r_minimal_action_clientというactionクライアントを作る手順を示していきます。
(actionサーバーも作る必要がありますが、同様の手順でできるので割愛します。)

なぜactionクライアントを作るのかというと、理由は2つあります。ひとつはサービスやパラメタのサンプルは公式が案内している[このリポジトリ](https://github.com/m-dahl/r2r_minimal_node/)で実装しているので、改めて作る意味がなかったからです。もうひとつは、シンプルにpub/subだけを実装してみるだけだとあまり面白くないからです。

### cargoでパッケージ(crato)を作る

```
cargo new r2r_minimal_action_client
```

r2r_minimal_action_client/下にsrc/mainl.rsとCargo.tomlが生成されます。

### r2r_cargo.cmakeを加える

[ここ](https://github.com/sequenceplanner/r2r/blob/master/r2r_cargo.cmake)からr2r_cargo.cmakeをダウンロードしてパッケージの直下に追加します。(wgetとかでダウンロードしてくる方がスマートかも。。)

詳しく読んでいなので、よくわかっていませんがどうもcolconとcargo間でのメッセージやパッケージ関連の処理を行っているようです。

### CMakeLists.txtとpackage.xmlを加える

CMakeLists.txtをパッケージの直下に追加してビルドの設定を記述します。
r2r_cargo.cmakeに記述されてるビルドスクリプトを使って依存パッケージのゴニョゴニョを何かしてるみたいです。

```cmake:CMakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(r2r_minimal_action_client)

find_package(ament_cmake REQUIRED)

if(NOT DEFINED CMAKE_SUPPRESS_DEVELOPER_WARNINGS)
     set(CMAKE_SUPPRESS_DEVELOPER_WARNINGS 1 CACHE INTERNAL "No dev warnings")
endif()

include(r2r_cargo.cmake)

# put ros package dependencies here.
r2r_cargo(std_msgs               # just to test that it works
          example_interfaces     # 今回使うActionが入ってる
          rcl                    # we need the c ros2 api
          rcl_action             # as of r2r 0.1.0, we also need the action api
          rmw_fastrtps_cpp       # (needed to build with RMW_IMPLEMENTATION=rmw_fastrtps_cpp)
          FastRTPS               # (needed to build with RMW_IMPLEMENTATION=rmw_fastrtps_cpp)
         )

# install binaries
install(PROGRAMS
  ${CMAKE_SOURCE_DIR}/target/release/${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME}
)

# we need this for ros/colcon
ament_package()

```

package.xmlをパッケージの直下に追加してビルドに必要な依存関係を記述します。

```xml:package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
  <name>r2r_minimal_action_client</name>
  <version>0.0.1</version>
  <description>Examples of a minimal r2r node</description>
  <maintainer email="takumi1988okamoto@gmail.com">Takumi Okamoto</maintainer>
  <license>MIT</license>
  <author>Takumi Okamoto</author>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>rcl</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>example_interfaces</build_depend>

  <exec_depend>rcl</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>example_interfaces</exec_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>
```

### main.rsにコードを記述する

今回は[r2r本家のexamples](https://github.com/sequenceplanner/r2r/blob/master/examples/action_client.rs)をそのまま使いました。src/main.rsにコピペします。

```rust:main.rs
use futures::executor::LocalPool;
use futures::future::FutureExt;
use futures::stream::StreamExt;
use futures::task::LocalSpawnExt;

use r2r::example_interfaces::action::Fibonacci;
use std::sync::{Arc, Mutex};

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let ctx = r2r::Context::create()?;
    let mut node = r2r::Node::create(ctx, "testnode", "")?;
    let client = node.create_action_client::<Fibonacci::Action>("/fibonacci")?;
    let action_server_available = node.is_available(&client)?;

    // signal that we are done
    let done = Arc::new(Mutex::new(false));

    let mut pool = LocalPool::new();
    let spawner = pool.spawner();

    let task_spawner = spawner.clone();
    let task_done = done.clone();
    spawner.spawn_local(async move {
        println!("waiting for action service...");
        action_server_available
            .await
            .expect("could not await action server");
        println!("action service available.");

        let goal = Fibonacci::Goal { order: 5 };
        println!("sending goal: {:?}", goal);
        let (goal, result, feedback) = client
            .send_goal_request(goal)
            .expect("could not send goal request")
            .await
            .expect("goal rejected by server");

        println!("goal accepted: {}", goal.uuid);
        // process feedback stream in its own task
        let nested_goal = goal.clone();
        let nested_task_done = task_done.clone();
        task_spawner
            .spawn_local(feedback.for_each(move |msg| {
                let nested_task_done = nested_task_done.clone();
                let nested_goal = nested_goal.clone();
                async move {
                    println!(
                        "new feedback msg {:?} -- {:?}",
                        msg,
                        nested_goal.get_status()
                    );

                    // 50/50 that cancel the goal before it finishes.
                    if msg.sequence.len() == 4 && rand::random::<bool>() {
                        nested_goal
                            .cancel()
                            .unwrap()
                            .map(|r| {
                                println!("goal cancelled: {:?}", r);
                                // we are done.
                                *nested_task_done.lock().unwrap() = true;
                            })
                            .await;
                    }
                }
            }))
            .unwrap();

        // await result in this task
        match result.await {
            Ok((status, msg)) => {
                println!("got result {} with msg {:?}", status, msg);
                *task_done.lock().unwrap() = true;
            }
            Err(e) => println!("action failed: {:?}", e),
        }
    })?;

    loop {
        node.spin_once(std::time::Duration::from_millis(100));
        pool.run_until_stalled();
        if *done.lock().unwrap() {
            break;
        }
    }

    Ok(())
}
```

asyncとかfuturesを使って非同期のコードがスッキリと書けています。executorみたいなのは書けないっぽい(未検証)ので、どうするかはちょっと検討の必要があるかもです。

## Cargo.tomlを記述する
以下のようにCargo.tomlを記述します。
特にfuturesは依存関係にあるので必須です。

```toml:Cargo.toml
[package]
name = "r2r_minimal_action_client"
version = "0.1.0"
authors = ["Takumi Okamoto <takumi1988okamoto@gmail.com>"]
edition = "2021"

[dependencies]
r2r = "0.6.3"
futures = "0.3.15"
tokio = { version = "1", features = ["full"] }
rand = "0.8.5"

```
(妥当なTOMLかは検証してないです。必要のないcrateも含まれているので、ご注意ください)

## ビルドする

```
colcon build --symlink-install
```

## 成果物

Githubに作ったものをまとめておいておきましたので、参考にしてください。
実行方法等はリポジトリのREADMEに記述しています。

https://github.com/MrBearing/r2r_minimal_action_node

## 所感
- 依存関係にあるソフトをインストールする必要がなかったのがちょっと感動
- サクッと動かせたので、個人的にはros2_rust使うよりもこちらのほうが良い
- [r2rのドキュメント](https://sequenceplanner.github.io/r2r/r2r/example_interfaces/index.html)を見ると、r2r::下にstd_msgなどがあるので、CMakeLists.txtとpackage.xmlで指定する必要ないかと思ったら、依存関係に含める必要があった。
- メッセージ関係を定義したときはCargo.lockと.cargoを削除してからリビルドしたほうが良い(今回はあまり関係ないが)
- ビルドの時間長い。。
- 何か小規模なプロジェクトで試してみたい

## 参考資料
- https://github.com/sequenceplanner/r2r
    - r2rの本家
    - /examples下のソースを持ってきた
- https://github.com/m-dahl/r2r_minimal_node
    - サービスを使う場合の参考になる
    - メッセージ定義をした場合の使い方の参考になる
    - このリポジトリのコピペでだいたいどうにかなりそう。