ROS2 Eloquentのパラメータ設定

nodeをパラメータ指定のlaunchファイルで動かすときに少し手こずったので，記録する．以下のコードはROS2のTutorialのコードにparameter部分だけ付け足したものである．

c++のコード(publisher側)

string_talker.cpp

# include <chrono>
# include <memory>

# include "rclcpp/rclcpp.hpp"
# include "std_msgs/msg/string.hpp"
using std::placeholders::_1;

using namespace std::chrono_literals;

class Talker : public rclcpp::Node
{
public:
    Talker() : Node("string_talker"), count(0)
    {
        string_param = this->declare_parameter("string_param", "defalt");
        parameter_event_sub = this->create_subscription<rcl_interfaces::msg::ParameterEvent>("/parameter_events", 10, std::bind(&Talker::update_parameters, this, _1));

        string_pub = this->create_publisher<std_msgs::msg::String>("string_topic", 10);
        timer = this->create_wall_timer(500ms, std::bind(&Talker::timer_callback, this));
    }

private:
    void timer_callback()
    {
        auto string_msg = std_msgs::msg::String();
        string_msg.data = string_param;
        // RCLCPP_INFO(this->get_logger(), "publishing : '%s'", string_msg.data.c_str());
        std::cout << "publishing : " << string_msg.data << std::endl;
        string_pub->publish(string_msg);
    }

    void update_parameters(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
    {
        if (event->node == this->get_fully_qualified_name())
        {
            this->get_parameter("string_param", string_param);
        }
    }

    std::string string_param;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr string_pub;
    rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub;

    rclcpp::TimerBase::SharedPtr timer;
    size_t count;
};

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

解説

string_param = this->declare_parameter("string_param", "defalt");

この行で，「この変数がパラメータですよ」と指定している．初期パラメータだけ変えられればいいと言う人はこれを使うだけでOK．int，floatをパラメータとして与えたいときは，パラメータの宣言はint64_tとdoubleで行う必要がある．サポートしているデータの型(ROS2 Parameter Design)

ROS2では，パラメータ設定がサービスを利用する形で実装されているため，サービスを用いて動的にパラメータの変更ができる．以下，動的に変更するための方法を説明する．

rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_event_sub;

parameter_event_sub = this->create_subscription<rcl_interfaces::msg::ParameterEvent>("/parameter_events", 10, std::bind(&Talker::update_parameters, this, _1));

この処理で，パラメータが更新されたときにupdate_parametersの関数がコールバックとして呼び出されるように設定している．

void update_parameters(const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
{
    if (event->node == this->get_fully_qualified_name())
    {
        this->get_parameter("string_param", string_param);
    }
}

ROS2ではノード毎にパラメータサーバが作成されるため，コールバック関数ではノードが合っているか確認し，パラメータを更新している．

c++のコード(subscriber側)

Tutorialの[Write the subscriber node](https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/#write-the-subscriber-node)とほとんど一緒なので折りたたみ

string_lister.cpp

# include <memory>

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

using std::placeholders::_1;

class Listener : public rclcpp::Node
{
public:
    Listener() : Node("string_listener")
    {
        subscroption = this->create_subscription<std_msgs::msg::String>("string_topic", 10, std::bind(&Listener::callback, this, _1));
    }

private:
    void callback(const std_msgs::msg::String::SharedPtr msg) const
    {
        // RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
        std::cout << "I heard : " << msg->data << std::endl;
    }
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscroption;
};

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

Tutorialとの変更点はMimimalSubscriberをListenerにしたくらい．内容の変更は皆無のはず．

launchのコード

string_topic.launch.xml

<launch>
    <node pkg="demo_parameters_cpp" exec="string_talker" >
        <param name="string_param" value="launch defalt" />
    </node>

    <node pkg="demo_parameters_cpp" exec="string_listener" output="screen" />
</launch>

paramのnameはパラメータで設定したときの名前，valueが与えたいパラメータの値．floatの型のものにintのvalue指定で渡すとエラーが出る．

launchのコード（yaml読み込み）

string_topic_with_yaml.launch.xml

<launch>
    <node pkg="demo_parameters_cpp" exec="string_talker">
        <param from="$(find-pkg-share demo_parameters_cpp)/parameters/string_topic.yaml" />
    </node>

    <node pkg="demo_parameters_cpp" exec="string_listener" output="screen" />
</launch>

yamlファイルは上記のようにfromで読み込む．

string_topic.yaml

string_talker:
    ros__parameters:
        string_param: "yaml defalt"

yamlの書き方は1行目に実行名( pkgではなくexec) ，2行目にros__parameters（これは固定），3行目からパラメータを書き始める．

最後に

以上のコードは，githubにまとめてある．

• 本記事ではstring型のパラメータだけだったが，githubにはint_array型のパラメータ設定もある．

• yamlを使うlaunchの際，CMakeListに少し追記する必要がある．

以上のことが気になったら，githubを参照してほしい．

参考

Tully Foote, Parameter API design in ROS

Michel Hidalgo, ROS 2 Launch XML Format

近藤 豊 様，ROS 2 EloquentのXML記法を使ったLaunchシステム

近藤 豊 様, 「ROS2ではじめよう次世代ロボットプログラミング」, p86

ros_turtlesim

ros2-tutorial