ROSを2つのSimpleControllerで使いたい

Japanese (日本語)
1

初めまして。研究でChoreonoid 2.0.0を使用させていただいています。

2つのSimpleControllerのinitialize関数で、rclcpp::init(0, nullptr);を呼び出していますが、この場合Choreonoidが強制終了し、次のようなエラーメッセージが表示されます。

terminate called after throwing an instance of 'rclcpp::ContextAlreadyInitialized'
  what():  context is already initialized
中止

次に、片方のSimpleControllerのinitialize関数をコメントアウトすると、次のようなエラーメッセージが表示されます。

terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidArgument'
  what():  failed to create guard condition: context argument is null, at ./src/rcl/guard_condition.c:65
中止 (コアダンプ)

2つのSimpleControllerで、ROSのノードをそれぞれ作成したい場合は、どのようにすればいいでしょうか？ご回答いただけると幸いです。よろしくお願いいたします。

2

追記です。initialize関数のあとには、ノードを作成する関数node = rclcpp::Node::make_shared("hoge_controller");を呼んでいます。

3

こんばんは、choreonoid_rosのROS 2版を開発している者です。

rclcppとchoreonoid pluginの初期化関数の記述が混在していてソースコードの状況を正しく理解できているのか怪しいのですが、一旦以下のようなソースコードを仮定します。
（お手元のソースコードの状況と私の理解が違いそうでしたら指摘いただけると幸いです）

class SimpleController1 : public SimpleController
{ 
public:
  virtual bool initialize(SimpleControllerIO* io) override 
  {
    // コメントアウトした
    // rclcpp::init(0, nullptr); 
    node = rclcpp::Node::make_shared("hoge_controller");
  }
};

class SimpleController2 : public SimpleController
{ 
public:
  virtual bool initialize(SimpleControllerIO* io) override 
  {
    rclcpp::init(0, nullptr);
    node = rclcpp::Node::make_shared("hoge_controller");
  }
};

このような状況の場合、ノードを作成する前に必ずrclcpp::initを呼び出すことが必要であることを考えると2つあるSimpleControllerのうちコメントアウトしていない方（SimpleController2）が先に初期化されることを保証する必要があります。

初期化の順番が守られなかった場合、2つ目のエラーを出すことがあるようです。
参考：URL

この場合、どうにかしてプラグインの初期化順を固定することも考えられますが、
以下のようにしてSimpleControllerの初期化順に関係なくrclcppを正しく初期化できるようにするのが良いと思います。

  virtual bool initialize(SimpleControllerIO* io) override 
  {
    if(!rclcpp::ok()) {
       rclcpp::init(0, nullptr);
    }
    node = rclcpp::Node::make_shared("hoge_controller");
  }
4

ご返信ありがとうございます！

質問時は、提供していただいたコード例の通りとなっており、rclcpp::ok()にて解決しました。
初歩的な質問でして、申し訳ないです。
的確なご指摘、ありがとうございました。

5

本件と関連して、choreonoid_ros（のROS 2版）におけるデフォルト（グローバル）コンテキストの初期化の扱いについて変更がありましたので、お知らせします。

従来はデフォルトコンテキストの初期化はchoreonoid_ros / ROS2プラグイン側では行っておらず、デフォルトコンテキストを使用する場合は自前で初期化を行う必要がありました。ご提示いただいたコードについても、このことをご理解の上で、シンプルコントローラ側で初期化をしていただいていました。

しかしシンプルコントローラはChoreonoidのシミュレーションプロジェクトにおいて同時に複数使用するものですし、そのようなものが各々でデフォルトコンテキストの初期化を行うようになると、競合を起こしてしまって収拾がつかなくなります。

それを避けるためにデフォルトではない独自のコンテキストをそれぞれ生成するという手法もあります。（ROS 2はROS 1とは異なり、ひとつのプロセス（実行ファイル）中で、複数のノードを生成できて、それぞれに独自のコンテキストを与えることも可能となっています。）ただしその場合はデフォルトのものを使う場合と比べてコードも複雑になってしまいますし、またコマンドライン引数などで与えられたROS 2用のオプションが反映されないといった問題もでてきます。そのようなこともあり、特に理由がなければ、デフォルトコンテキストを使う方が望ましいです。

そこで、choreonoid_rosにおいては、デフォルトコンテキストの初期化はシステム側で行うこととし、シンプルコントローラなどのユーザが作成するコードでは、初期化無しでデフォルトコンテキストを使うということに変更しました。既にポストしていただいたようなコードで運用されているかと思いますので、大変申し訳無いのですが、Choreonoidとchoreonoid_rosをアップデートしていただき、この仕様に従ってシンプルコントローラのコードを修正していただければと思います。

choreonoid_rosにおけるこの修正は以下のコミットになります。

このコミットの “choreonoid_ros2.cpp” の修正にあるように、実行ファイルの開始点で

rclcpp::init(argc, argv);

をしてデフォルトコンテキストを初期化し、終了時点で

rclcpp::shutdown();

として終了処理をしています。

ですのでシンプルコントローラ側ではこれらの初期化を行う必要はなくなりましたし、することもできなくなりました。そのままデフォルトコンテキストを使っていただければOKです。

この場合の実際のシンプルコントローラのコーディングについては、以下の「ROS 2モバイルロボットチュートリアル」を参考にしていただければと思います。

シンプルに書く場合は、以下のようなコードになります。

ただしこれだと少し処理効率が悪いところがあるので、以下のコードのようにexecutorを別スレッドで動かすのが望ましいです。

コードの詳しい解説がなくて申し訳ないのですが、とりあえずは上記のコードと同じように書いていただければOKかと思います。

1 Like