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

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

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

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

【参考URL】
YAMLファイルによるROS2のパラメータ設定 - Qiita
ROS2 YAML For Parameters - The Robotics Back-End
Using parameters in a class (Python) — ROS 2 Documentation
rclcpp::Node Class Reference - ROS 2 Docs
rclcpp::Parameter Class Reference - ROS 2 Docs
Do I always have to declare parameters? - ROS Answers

環境

Ubuntu20.04
ROS2:foxy

流れ

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

Yamlファイルの作成

params.yamlというファイルを作成します。
ファイル名や置く場所はどこでもいいのですが、慣習的にパッケージ直下にconfig/ディレクトリを作成してそこに保存しておくのが一般的のようです。

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:の下にパラメータを記載していきます。

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

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

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

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

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

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

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

#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ファイルが読み込まれなかったらこの値が使用されます。

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

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

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

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

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ファイルを指定することで設定できます。

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

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

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

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

ros2 run cpp_pubsub talker

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

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