はじめに
ROS2での開発をしていると複数センサの出力にセンサごとに異なる処理を施して３次元地図データのような一つの大きなデータにしたいときがあると思います（僕にはありました）．
例えば，Lidarとステレオカメラからそれぞれ取得したポイントクラウドをセンサ特性を考慮した前処理を加えて３次元地図構築処理に入力するといった具合です．
今回はそうした場合に対応するための方法を模索したのでそれを共有しようと思います．
方法
今回は具体的なコード（コンパイルしてすぐに使えるもの）ではなく要点だけ書いたコードで説明します．
以下のようなデータ処理を行う基底クラスProcessorBaseがあるとして，これを継承したProcessor1, Processor2, Processor3があるとします．
class ProcessorBase
{
public:
// センサデータに何らかの処理を行う関数
virtual void process(/*args*/) = 0;
/**
* コンストラクタ, デストラクタ,その他必要な関数と変数の宣言，定義
*/
};
class Processor1: public ProcessorBase
{
public:
void process(/*args*/) override
/**
* コンストラクタ, デストラクタ,その他必要な関数と変数の宣言，定義
*/
};
/**
* Processor2, 3の宣言,定義が続く
*/
そして，サブスクライブしたデータを上記のクラスを利用して処理をかけるノードを以下のように書きます．
今回はsensor_msgs::msg::PointCloud2を受け取るとします．
class ProcessorNode: public rclcpp::Node
{
public:
ProcessorNode(rclcpp::NodeOptions options)
private:
std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_point_clouds_;
void callbackPointCloud(const sensor_msgs::msg::PointCloud2& msg, const size_t processor_no);
std::vector<std::shared_ptr<ProcessorBase>> processors_;
};
ProcessorNode::ProcessorNode(rclcpp::NodeOptions options)
: rclcpp::Node("my_node", options)
{
processors_.push_back(std::make_shared<Processor1>(/*args*/));
processors_.push_back(std::make_shared<Processor2>(/*args*/));
processors_.push_back(std::make_shared<Processor3>(/*args*/));
for (size_t i=0; i<3; ++i)
{
sub_point_clouds_.push_back(
create_subscription<sensor_msgs::msg::PointCloud2>(
"input/point_cloud"+std::to_string(i), rclcpp::QoS(1),
[this, i](const sensor_msgs::msg::PointCloud2::UniquePtr msg)-> void
{
this->callbackPointCloud(*msg, i);
}
)
);
}
}
void ProcessorNode::callbackPointCloud(const sensor_msgs::msg::PointCloud2& msg, const size_t processor_no)
{
processors_[processor_no]->process(msg, /*args*/);
/**
* 他の処理
*/
}
/**
* その他関数の定義
*/
説明
ポイントはcallbackPointCloudをそのままサブスクライバのコールバック関数に登録するのではなくて，ラムダ式で定義されたコールバック関数の中でどのセンサのデータか（どのトピックが受け取ったか）を判別するための引数を追加してcallbackPointCloudを呼び出すということです．
直接std::bind(&ProcessorNode::callbackPointCloud, std::placeholders::_1, i)のようにサブスクライバーのコールバック関数を与えることもやってみたのですが，サブスクライバーに登録できるコールバック関数は受け取ったメッセージ１つだけを引数にとる関数だけに限定されているような感じがしました（ビルドしてエラーがでた程度の認識なので，詳しい実装内容をご存知の方がいらっしゃればコメントしてください）．
終わりに
今回は同じ処理を受け取るデータのソース（トピック名）で分けるようなサブスクライブ時のコールバック関数の登録について書いてきました．かなり抽象的なコードになっていますが要点はラムダ式を一度かませてどのトピックから受け取ったかを判別するための引数を追加するというところだけになっています．
また，filtersというROS2ライブライの実装方法を真似るとパラメータファイルからセンサ処理の方法や処理系の数を変えることができるのでソースが1つでも２つでもリビルドなしに実行できます（気が向いたらこのことについても記事を書いてみようかな）．
2024/8/1追記
複数のノードで１つのトピックにデータをパブリッシュすることができるそうです．もしもメッセージからframe_idなどを利用してどのノードからのトピックかが判別できるなら，このような面倒なことをしなくても良いかもです．
ここまで読んでいただきありがとうございました．ご参考になればと思います．
なにか改善点，不明点や誤りがあれば教えてください．