ROS2でyamlファイルからパラメータを設定する。

ROS2ではROS1と同様にノードにパラメータを設定することができます。
ROS2にはROS1であったようなパラメータサーバーはなく、ノード間でのパラメータのやり取りは個々のノードがパラメータサーバーの役割となり、やり取りをするようです。[ROS2のparameter概要 - Qiita](https://qiita.com/NeK/items/556a38782ce65bb81340)
パラメータの設定にはノード起動時にコマンドラインで指定する方法や、事前に用意したyamlファイルから読み込む方法があります。

この記事では個々のノード間でのパラメータのやり取りは考えずに、シンプルにyamlファイルからパラメータをノードに読み込む方法について解説していきます。

この記事は、ROS2での基本的なノードの作成等チュートリアルを完了している人が対象となります。


【参考URL】
[YAMLファイルによるROS2のパラメータ設定 - Qiita](https://qiita.com/NeK/items/15bf1e657d8d694592ed)
[ROS2 YAML For Parameters - The Robotics Back-End](https://roboticsbackend.com/ros2-yaml-params/)
[Using parameters in a class (Python) — ROS 2 Documentation](https://docs.ros.org/en/foxy/Tutorials/Using-Parameters-In-A-Class-Python.html)
[rclcpp::Node Class Reference - ROS 2 Docs](https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html)
[rclcpp::Parameter Class Reference - ROS 2 Docs](https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Parameter.html#a4ae8c4a4dfbaa300b741daa978bd8763)
[Do I always have to declare parameters? - ROS Answers](https://answers.ros.org/question/325960/do-i-always-have-to-declare-parameters/)

# 環境
Ubuntu20.04
ROS2:foxy

# 流れ
・Yamlファイルの作成
・パラメータを読み込むノードの作成
・実行確認

# Yamlファイルの作成
`params.yaml`というファイルを作成します。
ファイル名や置く場所はどこでもいいのですが、慣習的にパッケージ直下に`config/`ディレクトリを作成してそこに保存しておくのが一般的のようです。
```yaml
node_name1:
  ros__parameters:
    param1: 30        # int
    param2: 3.3      # double
    param3: "string" # string
    param4: true     # bool
    param5: [3, 33] # 配列の書き方
    param9: # ↓こういう配列の書き方もOK
      - "hoge"
      - "fuga"
      - "piyo"
```

`test_node1:`という記載でこのパラメータを使用するノードを指定しています。ここで指定されたノード以外はこのパラメータを使用できません。その後`ros__parameters:`の下にパラメータを記載していきます。

複数のノードのパラメータをいっぺんに書くこともできます。

```yaml
node_name1:
  ros__parameters:
    param1: 30        # int
    param2: 3.3      # double
    param3: "string" # string
    param4: true     # bool
    param5: [3, 33] # 配列の書き方
    param9: # ↓こういう配列の書き方もOK
      - "hoge"
      - "fuga"
      - "piyo"

node_name2:
  ros__parameters:
    param1: 30        # int
    param2: 3.3      # double
    param3: "string" # string
```

また、複数のノードから共通するパラメータを読み込みたいこともあるかと思います。そういう場合はワイルドカード`/**`を使います。

```yaml
/**:
  ros__parameters:
    shared_param1: 30       # int
    shared_param2: 3.3      # double
    shared_param3: "string" # string
```

このように`/**:`の下に書いたパラメータはどのノードからも読めるようになります。

# パラメータを読み込むノードの作成

パラメータを読み込んでその値をパブリッシュするノードを作成します。

```cpp
#include <chrono>
#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_;
    std::string text_;

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

    void timer_callback() {
        std_msgs::msg::String message;
        message.data = text_ + std::to_string(count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        publisher_->publish(message);
    }
public:
    MinimalPublisher() : Node("node_name1"), count_(0) {
        // 使用するパラメータの宣言
        declare_parameter("param1", 1);
        declare_parameter("param2", 1.3);
        declare_parameter("param3", "hoge");

        count_ = get_parameter("param1").as_int();
        text_ = get_parameter("param3").as_string();

        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;
}
```

このコードではコンストラクタでパラメータの取得を行なっています。
まず`declare_parameter("param1", 1)`で使用するパラメータを定義しています。`declare_parameter`の二つ目の引数はこのパラメータの初期値です。もしyamlファイルが読み込まれなかったらこの値が使用されます。

このようにパラメータの定義をしないとパラメータを使うことができません。もし、定義せずにパラメータを使いたかったら、ノードの継承時にこのようにオプションを指定してやると定義せずに使用できるようになります。

```cpp
MinimalPublisher() : Node("node_name1", rclcpp::NodeOptions().allow_undeclared_parameters(true)), count_(0) {
```


パラメータが定義されたら`get_parameter("param1").as_int()`というように型に合わせてパラメータを取得します。型の指定は`as_int()`以外にも

```cpp
as_bool()
as_int()
as_double()
as_string()
as_byte_array()
as_bool_array()
as_integer_array()
as_double_array()
as_string_array()
```

といったものがあります。[rclcpp::Parameter Class Reference - ROS 2 Docs](https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Parameter.html#a4ae8c4a4dfbaa300b741daa978bd8763)

pythonで書く場合はこちら
```python
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('node_name1')

        # 使用するパラメータの宣言
        self.declare_parameter('param1', 1)
        self.declare_parameter('param3', 'hoge')

        self.i = self.get_parameter('param1').get_parameter_value().integer_value
        self.text = self.get_parameter('param3').get_parameter_value().string_value


        self.publisher_ = self.create_publisher(String, 'topic', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        msg = String()
        msg.data = self.text + str(self.i)
        self.publisher_.publish(msg)
        self.get_logger().info('Publishing: "%s"' % msg.data)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
```

C++の場合とほとんど同じです。`__init__()`の中でパラメータの定義をしています。
パラメータを取得する部分がC++とちょっと毛色が違って、`self.get_parameter('param1').get_parameter_value().integer_value`といった感じになっています。型によってこの`integer_value`の部分を変えるのですが、それはそれぞれ以下のようになっています。単語をC++と統一してほしいですね。

```
bool_value
integer_value
double_value
string_value
byte_array_value
bool_array_value
integer_array_value
double_array_value
string_array_value
```

# 実行確認
パラメータは実行時にyamlファイルを指定することで設定できます。

```sh
ros2 run cpp_pubsub talker --ros-args --params-file /root/dev_ws/src/cpp_pubsub/config/params.yaml
```

このような形で`--ros-args --params-file`をつけてyamlファイルまでのパスを記載します。
実行されると、

```sh
[INFO] [1646042565.450613176] [node_name1]: Publishing: 'string30'
[INFO] [1646042565.950033552] [node_name1]: Publishing: 'string31'
[INFO] [1646042566.448344552] [node_name1]: Publishing: 'string32'
```

と表示されるでしょう。`'string30'`といったように数字がパラメータで指定した30から始まってるのがわかります。
ここでパラメータファイルを指定しないで実行してみましょう。

```sh
ros2 run cpp_pubsub talker
```

```sh
[INFO] [1646042677.348611422] [node_name1]: Publishing: 'hoge1'
[INFO] [1646042677.852959589] [node_name1]: Publishing: 'hoge2'
[INFO] [1646042678.349298673] [node_name1]: Publishing: 'hoge3'
```

パラメータが反映されておらず、コード上に記載した初期値が使用されているのがわかります。