Docker上に構築されたROS2で簡単なパッケージを作る。

Docker上に構築されたROS2を使って、簡単なパブリッシュサブスクライブのパッケージを作成します。
【参考】
[ROS 2 Documentation](https://docs.ros.org/en/foxy/index.html)

# 環境
【ホスト環境】
・MacBook pro Apple Silicon
・Docker: version 20.10.11
【Dockerゲスト環境】
・Ubuntu20.04
・ROS2: foxy
【開発言語】
・C++

# 流れ
・Dockerのインストール
・ROS2のコンテナ作成、実行
・パッケージ作成
・パブリッシャーノード作成
・サブスクライバーノード作成

# Dockerのインストール
今回使用しているPCはM1チップ搭載のMacBookproなので、[Docker Desktop for Apple silicon](https://docs.docker.com/desktop/mac/apple-silicon/)を参考にApple Silicon用のDockerをインストールします。
Intel Macや他のOSを使用している人は適宜、Dockerをインストールしてください。[Get Docker](https://docs.docker.com/get-docker/)

## rossetaのインストール
M1 Macの場合はrosettaを入れておいた方がパフォーマンスが上がるらしいので下記コマンドでインストールします。

```
softwareupdate --install-rosetta
```

## Docker Desktopのインストール
[Docker Desktop for Apple silicon](https://docs.docker.com/desktop/mac/apple-silicon/)からDocker.dmgをダウンロードして、インストールします。

# ROS2のコンテナ作成、実行
ROS2のコンテナを作成します。今回はfoxyバージョンを使用します。
ROS2のDockerイメージは公式に用意されているのでそれを使用します。

コンテナの作成、実行

```
docker container run -it --rm -v ~/dev_ws/:/root/dev_ws ros:foxy
```

このコマンドでROS2がインストールされているDocker環境に入ることが出来ます。
作ったものをホストPCに保存するために、上記のコマンドではホストPCの ~/dev_ws を Docker上の /root/dev_ws/ にマウントしてあります。これをすることでコードを書くのはホスト上で、ビルドと実行はDocker上で行うということができます。

# パッケージ作成
【参考】
[Writing a simple publisher and subscriber (C++)](https://docs.ros.org/en/foxy/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html)

Dockerのコンテナ上で作業します。

まずはマウントした作業スペースに移動しましょう。

```
cd ~/dev_ws
```

`src`というディレクトリを作成して移動します。

```
mkdir src
cd src
```

そしたらパッケージを作成します。

```
ros2 pkg create --build-type ament_cmake cpp_pubsub
```
`cpp_pubsub`というのが自分で名付けるパッケージ名です。

実行すると`src`以下にファイルが作られ下記のようなファイル構造になります。

```
dev_ws
└── src
    └── cpp_pubsub
        ├── CMakeLists.txt
        ├── include
        │   └── cpp_pubsub
        ├── package.xml
        └── src
```

# パブリッシャーノード作成
## ノードの作成
`dev_ws/src/cpp_pubsub/src`以下にパブリッシャーノードのコードを書き配置します。
`dev_ws/src/cpp_pubsub/src/publisher_member_function.cpp`を作成して下記のように編集します。ファイルの作成、編集はDocker上、ホストPC上どっちで行っても構いません。

```cpp
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node {
    size_t count_;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;

    void timer_callback() {
        auto message = std_msgs::msg::String();
        message.data = "Hello, world! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
public:
    MinimalPublisher() : Node("minimal_publisher"), count_(0) {
        count_ = 0;
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
        timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalPublisher>());
    rclcpp::shutdown();
    return 0;
}
```

このノードは0.5秒に一回`Hello, world! 0`といった感じの文字列をパブリッシュするノードとなっています。

ROS1ではコードの書き方はだいぶ自由でしたが、ROS2ではある程度決められているようです。

`#include "rclcpp/rclcpp.hpp"`はROS2の基本的な機能をインクルードします。
`#include "std_msgs/msg/string.hpp"`はパブリッシュするメッセージの型をインクルードしています。

ノードはクラス単位で作成します。
クラス作成時に`rclcpp::Node`を継承することでそのクラスがノードとして定義され、コンストラクタの初期化子でノードの名前を決定します。
クラスのメンバを見ると`timer_`と`publisher_`があります。
それぞれコンストラクタで初期化されています。
`publisher_`はノードでパブリッシュする型やトピック名、バッファ等を指定しています。
`timer_`はノードで定期実行してほしいコールバック関数やその実行周期を指定しています。このように初期化しておくことで、ノード実行時に自動的にコールバック関数を周期実行してくれます。
コールバック関数はラムダ式で指定することもできます。その場合コンストラクタで行うtimer_の初期化は下記のようになります。

```cpp
timer_ = this->create_wall_timer(500ms,
    [this]() {
        auto message = std_msgs::msg::String();
        message.data = "Hello, world! " + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
);
```

`main`関数では作ったノードのインスタンス化と実行を行なっています。

## package.xmlの編集
`package.xml`に依存関係を記入します。
`cpp_pubsub/`ディレクトリ下にある`package.xml`の`<buildtool_depend>ament_cmake</buildtool_depend>`の次の行に下記を追記します。

```xml
<depend>rclcpp</depend>
<depend>std_msgs</depend>
```
これは、コードの実行時にパッケージに`rclcpp`と`std_msgs`が必要であることを宣言しているらしいです。

`package.xml`全体は以下のようになります。

```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>cpp_pubsub</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>std_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

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

## CmakeLists.txtの編集
`cpp_pubsub/`ディレクトリ下にある`CMakeLists.txt`を編集します。
`find_package(ament_cmake REQUIRED)`の下に下記を追記します。

```cmake
# 依存関係の追記
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# talkerという名前を付けて、ros2 run コマンドを使用してノードを実行できるようにする。
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

# ros2 run コマンドが実行可能ファイルを見つけられるようにする。
install(TARGETS
  talker
  DESTINATION lib/${PROJECT_NAME})
```

`CmakeLists.txt`全体は以下のようになります。

```cmake
cmake_minimum_required(VERSION 3.5)
project(cpp_pubsub)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# 依存関係の追記
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# talkerという名前を付けて、ros2 run コマンドを使用してノードを実行できるようにする。
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)

# ros2 run コマンドが実行可能ファイルを見つけられるようにする。
install(TARGETS
  talker
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
```

## ビルド
ここまできたらビルドしてみましょう。
`dev_ws/`に移動して下記コマンドを実行します。

```
colcon build
```

こんな感じの表示がされたらビルド成功です。

```
Starting >>> cpp_pubsub
Finished <<< cpp_pubsub [13.9s]

Summary: 1 package finished [14.2s]
```

## 実行確認
作ったノードを実行してみましょう。
まずは、実行のためのセットアップをします。
ビルドが成功していれば`dev_ws`下に`install/setup.bash`があるはずなのでそれを`source`します。

```
source ./install/setup.bash
```

そしたらノードを起動します。

```
ros2 run cpp_pubsub talker
```

こんなのが出力されていたらノードが起動できています。

```
[INFO] [1642666863.149042250] [minimal_publisher]: Publishing: 'Hello, world! 0'
[INFO] [1642666863.654240584] [minimal_publisher]: Publishing: 'Hello, world! 1'
[INFO] [1642666864.154278667] [minimal_publisher]: Publishing: 'Hello, world! 2'
[INFO] [1642666864.652825876] [minimal_publisher]: Publishing: 'Hello, world! 3'
```

このノードは`'Hello, world! 0'`といったストリングのメッセージを0.5秒おきにパブリッシュしています。
このパブリッシュされたメッセージを確認してみましょう。
ROS2にはパブリッシュされているメッセージを確認するコマンドがあります。
新しくターミナルを立ち上げてそこで確認してみましょう。

新しく立ち上げたターミナルでdockerを起動します。

```
docker container run -it --rm -v ~/dev_ws/:/root/dev_ws ros:foxy
```

dockerに入ったら下記のコマンドでどんなトピックがパブリッシュされているか確認してみましょう。

```
ros2 topic list
```

下記のように今流れているトピックの一覧が見れます。

```
/parameter_events
/rosout
/topic
```

`talker`がパブリッシュしているトピックは`/topic`なので、下記のコマンドで内容を確認します。

```
ros2 topic echo /topic
```

うまく見れればこんな風に出力されているのが確認できます。

```
data: Hello, world! 11
---
data: Hello, world! 12
---
data: Hello, world! 13
---
```

ノードの終了、トピック確認の終了はCtrl+Cでできます。

# サブスクライバーノード作成
パブリッシャーノードでパブリッシュしたデータをサブスクライブするノードを作成します。
基本的にパブリッシャーノードと同じです。

`dev_ws/src/cpp_pubsub/src`以下にサブスクライバーノードのコードを書き配置します。
`dev_ws/src/cpp_pubsub/src/subscriber_member_function.cpp`を作成して下記のように編集します。

```cpp
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node {
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

    void topic_callback(const std_msgs::msg::String::SharedPtr msg) const {
        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }
public:
    MinimalSubscriber() : Node("minimal_subscriber") {
        subscription_ = this->create_subscription<std_msgs::msg::String>("topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));
    }
};

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MinimalSubscriber>());
    rclcpp::shutdown();
    return 0;
}
```

このサブスクライバーノードは、`topic`というトピック名で流れてきたメッセージをサブクスライブしてコンソールに表示するノードとなっています。

パブリッシャーノードの時と違ってメンバに`subscription_`があります。
コンストラクタで初期化されておりサブスクライブする型やトピック名、バッファ、そしてメッセージをサブスクライブした時に実行するコールバック関数を指定しています。このように初期化しておくことで、ノード実行後、トピックを受信するたびにコールバック関数を実行してくれるようになります。
コールバック関数はラムダ式で指定することもできます。その場合コンストラクタで行う`subscription_`の初期化は下記のようになります。

```cpp
subscription_ = this->create_subscription<std_msgs::msg::String>("topic", 10,
    [this](const std_msgs::msg::String::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    }
);
```

## CmakeLists.txtの編集
パブリッシャーノードの時と同様に`cpp_pubsub/`ディレクトリ下にある`CMakeLists.txt`を編集します。
パブリッシャーノードで記載したものにさらに追記する形となるので、追記するのは下記の２行と

```cmake
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)
```

下記のように`install`に`listener`を追記するだけです。

```cmake
install(TARGETS
  talker
  listener
  DESTINATION lib/${PROJECT_NAME})
```

`CmakeLists.txt`全体は下記のようになります。

```cmake
cmake_minimum_required(VERSION 3.5)
project(cpp_pubsub)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# 依存関係の追記
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# talkerという名前を付けて、ros2 run コマンドを使用してノードを実行できるようにする。
add_executable(talker src/publisher_member_function.cpp)
ament_target_dependencies(talker rclcpp std_msgs)
add_executable(listener src/subscriber_member_function.cpp)
ament_target_dependencies(listener rclcpp std_msgs)

# ros2 run コマンドが実行可能ファイルを見つけられるようにする。
install(TARGETS
  talker
  listener
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()
```

## ビルド
パブリッシャーノードと同様にビルドしてみましょう。
`dev_ws/`に移動してビルドを実行します。

```
cd　~/dev_ws
colcon build
```

`colcon build`はパッケージ全体をビルドしてくれます。
つまり、もしパブリッシャーノードに変更があった場合はそっちも同時にビルドしてくれます。

ついでに`source install/setup.bash`しておきましょう。
新しくビルドしてパッケージに変更があったり、docker立ち上げた直後とかは`source install/setup.bash`しとくのがいいっぽいです。

## 実行確認
まずはパブリッシャーノードを起動しましょう。dockerを立ち上げます。

```
docker container run -it --rm -v ~/dev_ws/:/root/dev_ws ros:foxy
```

パブリッシャーノードを起動します。

```
cd ~/dev_ws
source install/setup.bash
ros2 run cpp_pubsub talker
```

別のターミナルを起動してまたdockerを立ち上げます。

```
docker container run -it --rm -v ~/dev_ws/:/root/dev_ws ros:foxy
```

サブスクライバーノードを起動します。

```
cd ~/dev_ws
source install/setup.bash
ros2 run cpp_pubsub listener
```

こんな風に出力されれば成功です。

```
[INFO] [1642670068.610582344] [minimal_subscriber]: I heard: 'Hello, world! 121'
[INFO] [1642670069.109768969] [minimal_subscriber]: I heard: 'Hello, world! 122'
[INFO] [1642670069.607692928] [minimal_subscriber]: I heard: 'Hello, world! 123'
```