Open3

static ROSという可能性

こんなの絶対誰か考えてるし、何ならどこかの会社の内製フレームワークで似たようなことしてるだろと思いつつ….

ROS1が辛い(ROS2は詳しくないが多分あんまり変わらなそう).C++という静的言語(なお型安全性)を使っているのに、異様に気を使う気がする.
一つの原因としては、パラメータやトピックが必要以上に動的になっているからだと思う.

少し前にTier IVのリポジトリを覗いたときに、 interface.yamlという以下のような形式のファイルがノードによってはあって、「作りたくなる気持ちわかるー」と一人で納得したりもした.

- name: dpm_ocv
  publish: [/obj_X/image_obj]
  subscribe: [/config/obj_X/dpm, /image_raw]
- name: dpm_ttic
  publish: [/obj_X/image_obj]
  subscribe: [/config/obj_X/dpm, /image_raw]
- name: kf_track
  publish: [/obj_X/image_obj_tracked]
  subscribe: [/config/obj_X/kf, /image_raw, /obj_X/image_obj_ranged]
- name: klt_track
  publish: [/obj_X/image_obj_tracked]
  subscribe: [/config/obj_X/klt, /image_raw, /obj_X/image_obj_ranged]
- name: obj_reproj
  publish: [/obj_X/obj_label, /obj_X/obj_label_marker]
  subscribe: [/obj_X/image_obj_tracked, /current_pose, /projection_matrix, /camera/camera_info]
- name: range_fusion
  publish: [/obj_X/image_obj_ranged]
  subscribe: [/config/obj_X/fusion, /obj_X/image_obj, /points_image]

from: https://github.com/Autoware-AI/core_perception/blob/master/vision_ssd_detect/interface.yaml

ただ、これは単なるドキュメントに過ぎない.コメントの記述が古くなりむしろ有害になるということがあるが、それと同じことに陥るリスクがある.
そういう意味では、C++のコードからわかるといいんだけど、コンパイラがやっているような字句解析すると思うとまあ現実的ではない.
じゃあ逆にというわけで、こうした設定ファイルからコードを生成するというアプローチがある.当然、キューサイズとかも設定ファイルに書いてあげる必要はあるわけだが、情報さえ十分あればコード生成自体はそう難しくはない.
その方向で考えてみたい.

まずは簡単なパラメータから.
msgファイルとかに寄せて、以下のようなファイルを定義して、

SampleNode.param
double min_ang_limit
double max_ang_limit
:

そこから以下のような構造体の定義を生成することもそう難しくないはず.

struct SampleNodeParam
{
  double min_ang_limit;
  double max_ang_limit;
  :
};

正直これでは表現力が足りないと思うので、以下のような記述に対応してもいいだろう.

MoreComplex.param
Internal[] foo
AnotherInternal[] bar
[Internal]
int foo
double bar
[AnotherInternal]
string foo
int bar
struct MoreComplexPram
{
  struct Internal
  {
    int foo;
    double bar;
  };
  struct AnotherInternal
  {
    std::string foo;
    int bar;
  };
  std::vector<Internal> foo;
  std::vector<AnotherInternal> bar;
};

デフォルト値や簡単な事前条件を設定できるようにしたり、ros::NodeHandleを受け取って値を取ってくる関数だって多分作れるし、これもそう難しくはない.

(一番難しいというか自明でないのはフォーマットと、事前条件等で失敗したときにどう伝えるかとか.RustのOptionalやResultが欲しくなる)

dynamic_reconfigureとどう折り合いをつけるかというのも難しいかも.UIさえなんとかできれば、dynamic_reconfigureのような機能を提供するという手段もありえるか.

面倒な方.
そもそもこの考えが浮かんだのは以下の記事を読んだのがきかっけ.

https://t-wada.hatenablog.jp/entry/design-for-testability

自分が昔Webやっていたときは、Ruby on Railsを使っていたので、timecopという時間を改竄できるライブラリを使ってテストを書いていた.それからはWeb業界からは遠ざかり、時刻が関わるようなテストを考えるような機会もなく、そこで知識が止まっていた.(Rubyはメタプログラミングとか色々できる方の言語なので、そうでないJavaとかはどうしているのだろうとはうっすら思っていた)

だが、t-wadaさんの記事を読んで、他にも色々なアプローチがあるのかと非常にためになった.特に最後の「アプローチ7: 現在時刻へのアクセスを行うインターフェイスを抽出」はt-wadaさんが以下のように言及しているように、おそらく一押しなのだろうが、これがROSにも適用できるのではと考えた.

私はコードの世界、オブジェクト達の世界から外部環境にアクセスする界面の部分にインターフェイスを作成し、テストダブルで置き換えられるようにするという設計判断をよく行います。

以下のようにROSに関係する変数を直接持たせてしまいがちだが、そうしてしまうと、ROSと密結合になりテストもしづらい.

class Node
{
  Node() : nh_(), pub_{nh_.advertise<std_msgs::String>("chatter", 100)}, timer_{} {}

private:
  void timerCallback(ros::TimerEvent &)
  {
    std_msgs::String msg{};
    msg.data = "aaaaa";
    pub_.publish(msg);
  }

  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Timer timer_;
};

そうではなく、ros::Publisher等は間接的に持つようにし、publish等もEnvironmentが提供する関数を通して行う.

class Environment
{
public:
  virtual ~Environment() = 0;

  virtual void publish_chatter(const std_msgs::String::ConstPtr & topic) = 0;
};

class ROSEnvironment : public Environment
{
public:
  ROSEnvironment() : nh_()
  {
    pub_ = nh_.advertise<std_msgs::String>("chatter", 100);
  }

  void publish_chatter(std_msgs::String && msg) { pub_.publish(msg); }

  ros::Time now() { return ros::Time::now(); }  // ros::Timeもこの辺りで持つ.

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Timer timer_;
};

class Node
{
public:
  Node(std::shared_ptr<Environment> env) env_(env) {
    // timerCallbackも何とかしてEnvironmentに伝える
  }

private:
  void timerCallback(ros::Event &)
  {
    std_msgs::String msg;
    msg.data = "aaaaa";
    env_->publish_chatter(std::move(msg));
  }

  std::shared_ptr<Environment> env_;
};

すると、テスト時はEnvironmentを差し替えることで、ros::Timeを決められた値にしたり、publishが呼ばれるときに、適当な領域に退避させれば、簡単に中身を使ってテストができる.もっと抽象化頑張ればROS1とROS2の差し替えも原理的には可能(機能の違いによるあれこれはあるだろうが).

ただ、大きな問題があって、コード量が増大する.t-wadaさんもデメリットとして以下のように指摘しているように、どう考えてもやりすぎ.

このアプローチのデメリットは、場合によっては「やりすぎ」を招きやすいことです。

ただし、設定ファイルからEnvironmentROSEnvironmentを生成できるのならその限りではなく、設計として受け入れられるはず?

とはいえ、publisher, subscriber, service, (action, ) TF周りそして、message_filtersのような存在等色々考えることはあるので結構大変そう.

ログインするとコメントできます