###概要
お前ハマってばっかりやな
研究室の先輩方が取り組まれていた自動運転ロボット開発コンテストに関わることになったので、とりあえず画像を出版してホストで見ようとしたら予定外にハマってえらく時間がかかってしまいました。
その時起こった現象と解決策をメモ書きとして残します。
（ROS2共通の問題なのか、Ultra96上のROS2固有の問題なのか・・・）
###環境
ボード:Ultra96
OS: Ubuntu 18.04
ROS Distro: Dashing
###起こった現象
元々、同一デバイス内でトピック経由の画像送受信が行われていました。
ロボットのデバッグ・検証のため、同一ネットワーク内のPCから画像を出版しているトピックを購読しようとしたところ、
- そもそも画像が送られてこない
- 画像がくるが遅延がとんでもなく、だいぶ前の画像が飛んでくる
- 送信ノードが落ちる
などの問題が発生してしまいました。
デバイス内の画像出版は問題なく行われていたにもかかわらず、デバイス外に出版しようとすると起こりました。
###送信ノードが落ちる問題
画像の送信を始めてしばらくすると、このようなエラーが出て送信側ノードが落ちてしまいました。
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
what(): failed to publish message: cannot publish data, at /tmp/binarydeb/ros-dashing-rmw-fastrtps-shared-cpp-0.7.5/src/rmw_publish.cpp:52, at /tmp/binarydeb/ros-dashing-rcl-0.7.6/src/rcl/publisher.c:257
以下のサイトに情報がありました。
Exception sending message over network - ros2/rmw_fastrtps
どうもデフォルトで設定されているDDSであるFast-RTPS固有のバグみたいです。
このため、DDSをFast-RTPSからOpenSpliceに切り替えてやると良いみたいです。
OpenSpliceのインストールおよび切り替え方法は以下のサイトに掲載されていました。
Installing ROS2 via Debian Packages - ROS index
OpenSpliceがインストールされていない場合、使用しているdistroに合わせてaptで入れてやると良いです。
sudo apt update
sudo apt install ros-<ROS_DISTRO>-rmw-opensplice-cpp
今ぼくはDashingを使用しているので、<ROS_DISTRO>の部分にはdashingといれます。
あとは、環境変数をOpenSpliceに変更すると、DDSがOpenSpliceに切り替わります。
export RMW_IMPLEMENTATION=rmw_opensplice_cpp
めんどくさければ.bashrcに追記してもいいかもしれません。
ちなみに、デフォルトのFast-RTPSに戻すときは環境変数RMW_IMPLEMENTATIONの値をrmw_fastrtps_cppにするか、あるいは空にすると良いです。
###画像の遅延がめちゃくちゃでかい問題
OpenSpliceで画像がくるようになったのはいいものの、とにかく遅延が激しく、めちゃくちゃ前に撮られたはずの画像が送られてくるという状況でした。
画像サイズがでかいから遅延が発生しているのだろうということで、送信画像の圧縮を試みました。
//inputImageはcv::Mat型で、圧縮する入力画像が格納されているものとします。
//webcam_pub_はCompressedImageを出版するパブリッシャノードとします。
auto webcam_pub_image = std::make_shared<sensor_msgs::msg::CompressedImage>();
std::vector<int> param(2);
param[0] = cv::IMWRITE_JPEG_QUALITY;
param[1] = 30; //画像の品質（0-100,小さいほど高圧縮低画質）
cv::imencode(".jpg", inputImage, webcam_pub_image->data, param);
webcam_pub_->publish(webcam_pub_image);
出版データにはsensor_msgs::msg::CompressedImage型を使用しています。
（これもしかしたらわざわざCompressedImageにしなくても普通にImageでええんやないか・・・データ構造ほぼ同じやし・・・
param[1]に圧縮率を指定していて、ここの数字を小さいものにすることで高圧縮低サイズの画像を生成しています。
これをすることでフレームレートが爆向上しました。なぜここにもっと早く辿り着けなかったのか。
###圧縮画像を受信する
画像を圧縮して送信したので、受信側も圧縮画像に対応した実装でなければなりません。
ROS2において出版画像を閲覧するためには、ros2でのプログラム内 image_tools パッケージのshowimageノードが使えます。
(https://github.com/ros2/demos/blob/master/image_tools/src/showimage.cpp)
導入するためには、ROSディストリビューションにあったブランチ（僕の場合はdashing)を選択し、ソースをcloneもしくはdownloadし、ダウンロードしたものの中からimage_toolsパッケージをROSワークスペースに入れてビルドします。
ビルドする前に、圧縮画像を見られるようにするための改変をshowimage.cppに施します。
まず、CompressedImage型を扱えるようにします
//ヘッダをインクルード
//l25
// #include "sensor_msgs/msg/image.hpp" ->削除
#include "sensor_msgs/msg/compressed_image.hpp" //<-追加
// Image->CompressedImageに変更
//l59
const sensor_msgs::msg::CompressedImage::SharedPtr msg, bool show_camera, rclcpp::Logger logger)
//l136
auto callback = [show_camera, &node](const sensor_msgs::msg::CompressedImage::SharedPtr msg)
//l144
auto sub = node->create_subscription<sensor_msgs::msg::CompressedImage>(
ついで、コールバック関数内で圧縮画像をデコードするよう処理を変更します
//l66-
/*
cv::Mat frame(
msg->height, msg->width, encoding2mat_type(msg->encoding),
const_cast<unsigned char *>(msg->data.data()), msg->step);
if (msg->encoding == "rgb8") {
cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
}
*/ //上記部分全てコメントアウト
cv::Mat frame = cv::imdecode(msg->data,-1);// <ー追加
あとは、ビルドして実行します。
showimageは起動時にコマンドラインから購読トピックを指定することができます。
(この購読トピック変更はshowimageのログ出力には反映されません。ros2 topic listなどから確認できます。）
colcon build
ros2 run image_tools showimage /image:=webcam_image
カメラからの画像をリアルタイム（？）に見ることができます。
以下の画像のように、複数showimageノードでそれぞれ異なるトピックを購読して、同時に複数台のカメラからの映像を見ることもできました。
###まとめ
なぜ画像を見るだけでこれだけ消耗してしまったのか。