大変ご無沙汰しております．最近ようやくROSとChoreonoidを組み合わせて使い始めました．
さて，BodyROSプラグインでは，深度カメラのモデルに対してPointCloud2をパブリッシュしますが，PointCloud2は通信量が大きいので，代わりに深度画像を送ろうと思います．
bodyファイルのリファレンスマニュアルを見ると，Cameraノードのformatには5種類ありますが，実行してみると，COLOR以外は同じ内容を出力しているようで，深度画像を得ることができませんでした．
https://choreonoid.org/ja/documents/latest/handling-models/modelfile/yaml-reference.html#camera
src/BodyPlugin/GLVisionSimulatorItem.cppの中で点群データを作ったり，BodyROSItem.cppの中で各種のデータをパブリッシュしていることはわかりましたが，どこに組み込むのがいいでしょうか？
深度情報について、現在BodyROSItemでサポートされている以外のROSのメッセージ型を使いたいということですね。
これについては、現状ではシンプルコントローラを使って自前で実装するのが一番早いかと思います。
この方法について、以下の「 ROS版Tankチュートリアル」で説明していますので、これをご参照の上でトライしてみていただけますでしょうか。
https://choreonoid.org/ja/documents/latest/ros/tank-tutorial/index.html
BodyROSItemについては、現状では作り込みが足りないところもあるかと思います。今回は上記方法を試していただいた上で、BodyROSItemに関するご提案などもありましたら、お知らせいただければと思います。
返信ありがとうございます．
まず，確認ですが，現状では，BodyROSItemは深度画像は出力していませんよね？深度情報は，PointCloud2型のメッセージしかパブリッシュしていないと認識していますが，あっていますか？
ROSでは，image_transportというライブラリが，OpenCVで言うところの16UC1か32FC1の深度画像を与えれば，「基本トピック名/compressedDepth」というトピック名でPNG圧縮したメッセージをパブリッシュしてくれますので，それを利用しようと考えています．
その機能をBodyROSItemに追加することを考えています．BodyROSItem::updateRangeVisionSensor()を真似して，1画素ごとにz座標を取り出して深度画像を作ることはできましたが，もっと効率の良い方法はないものかと考えています．以下の行で深度画像を得ているようなので，これを直接使うようなことはできないかものかと…
現状では，「カメラ名/depth」というトピックを作り，以下のようなコードをBodyROSItem.cppに追加して，image_transport利用して「カメラ名/depth/compressedDepth」というPNGで圧縮したメッセージをパブリッシュできています．しかし，果たしてこのやり方でいいか自信がありません．また，Cameraノードの各formatに対してどのような仕様にすべきかということもわかりません．
パブリッシャの登録
range_vision_sensor_publishers_.resize(rangeVisionSensors_.size());
range_vision_sensor_depth_publishers_.resize(rangeVisionSensors_.size());
for (size_t i=0; i < rangeVisionSensors_.size(); ++i) {
if (RangeCamera* sensor = rangeVisionSensors_[i]) {
std::string name = sensor->name();
std::replace(name.begin(), name.end(), '-', '_');
range_vision_sensor_publishers_[i] = rosnode_->advertise<sensor_msgs::PointCloud2>(name + "/point_cloud", 1);
sensor->sigStateChanged().connect(boost::bind(&BodyROSItem::updateRangeVisionSensor,
this, sensor, range_vision_sensor_publishers_[i]));
ROS_INFO("Create RGBD camera %s (%f Hz)", sensor->name().c_str(), sensor->frameRate());
range_vision_sensor_depth_publishers_[i] = it.advertise(name + "/depth", 1);
sensor->sigStateChanged().connect(boost::bind(&BodyROSItem::updateRangeVisionSensorDepth,
this, sensor, range_vision_sensor_depth_publishers_[i]));
ROS_INFO("Create RGBD camera %s (%f Hz) depth", sensor->name().c_str(), sensor->frameRate());
}
}
パブリッシュ処理
void BodyROSItem::updateRangeVisionSensorDepth(RangeCamera* sensor, image_transport::Publisher& publisher)
{
if (publisher.getNumSubscribers() == 0) {return;}
sensor_msgs::Image vision;
vision.header.stamp.fromSec(io->currentTime());
vision.header.frame_id = sensor->name();
vision.width = sensor->resolutionX();
vision.height = sensor->resolutionY();
//32bit 浮動小数点数の深度画像
vision.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
vision.is_bigendian = false;
const int point_step = 4;
vision.step = point_step * vision.width;
vision.data.resize(vision.step * vision.height);
const std::vector<Vector3f>& points = sensor->constPoints();
unsigned char* dst = (unsigned char*)&(vision.data[0]);
for (size_t j = 0; j < points.size(); ++j) {
float z = - points[j].z();
std::memcpy(&dst[0], &z, point_step);
dst += point_step;
}
publisher.publish(vision);
}
この件なのですが、基本的な方針として、まずBodyROSItemとは別に、独自にSimpleControllerとして実装して試してみていただけますでしょうか。
恐縮ながら、BodyROSItemについては公式の実装になるので、改良された場合でも（他への影響や他からの要望との調整などを考慮して）すぐにmasterに取り込めるとは限らないところがあり、またWRSに向けて作業されているとすると、WRSでROSプラグインの扱いがどうなるか分からないところがあるので、上記をおすすめしたい次第です。WRSについては各チームごとに恐らくやりたいことがあって、競技直前までそれぞれ改良を続ける可能性があるので、そのような段階のものを公式のプラグインに入れて共有することは難しく、各チームごとに各自のコントローラ側で実装するのがよいのではないかと思っています。公式のプラグインについてチームごとに派生版のプラグインがあって、チームごとに異なる競技環境を運営側が用意することになると、運営側での手間や混乱が生じるので、恐らく認められないのではないかと…。（これは競技委員公式の見解ではなくて、あくまで私の個人的見解です。）
シンプルコントローラで実装する場合も、BodyROSItemに追加実装するのとさほど手間は変わらないというか、むしろ自由に書ける分実装も楽かと思っております。そのあたりは上記の「ROS版Tankチュートリアル」をご参照ください。（ちなみにこちらカメラ画像のPublishがまだ書けていなくて、その点申し訳ないのですが、カメラ画像をPublishするシンプルコントローラのソースコードはリポジトリに含まれていますので、そちらもご参照いただければと思います。）
その上で、今回ご作成いただく実装について、うまくいくものができましたら、あらためてその内容をBodyROSItemに取り込む提案をしていただけると、大変有り難く思います。そのような要望についてはできる限り対応させていただきたく思っています。
以上、まずどこに実装するかの方針について当方の希望を説明させていただきました。
こちらについては、シミュレータ内部の実装になりますので、ここから直接データを取り出してROSのメッセージ型にコピーするようなことは難しいです。ユーザープログラム側でできることは、対応するセンサオブジェクトがメンバ変数等で保有するデータから取り出すことのみとなります。
Cameraデバイスについて、フォーマットがDEPTHやCOLOR_DEPTHの場合でも、深度画像に直接対応するデータはCameraデバイスからは取得できないということですね？そちらについては当方でも確認してみます。できないということであればシミュレータとしては手落ちになるかと思うので、深度画像の生成についても検討してみます。
実装を見返しました。フォーマットがDEPTHやCOLOR_DEPTHの場合、内部的にはRangeCameraクラスが使われることになりますが、確かにこのクラスには3次元の深度データ（ポイントクラウドデータ）はありますが、深度画像のデータはないですね。
RangeCameraクラスに別途Image型のメンバも追加して、そこにグレイスケール画像として深度情報が入るようにすればよろしいですよね。深度画像で格納するかポイントクラウドで格納するかはどこかで選択できるようにして。
このあたり、最初に実装した際に恐らく深度画像という概念が抜けていたようです。でもKinect等のRGBDカメラの場合、通常はデバイスから得られる情報は深度画像になるということでよろしいでしょうか？だとしたらこのクラスもそのようにしておくべきですね。
ちなみにRGBDカメラの場合、Dの情報も（RGBと共に）ひとつの画像データに含まれるものでしょうか？それともDの深度情報は別の画像データに分けて出力されるものでしょうか？そのあたり詳しくないのですが、どちらがより一般的かについて、どなたか詳しい方いらっしゃいましたら教えてください。
あとはいつ実装できるかですが…。これについては現時点で実現は保証はできませんが、とりあえず検討してみます。やるとすればWRSに間に合うタイミングでやることが必要ですよね？
ちなみに実装については、RGBとDで画像データを分けるのでよければ、まずsrc/Bodyで定義されているRangeCameraクラスにImage型のメンバ変数を追加した上で、升谷先生が上記でご検討されているGLVisionSensorItemの関数内で、glReadPixelsで取得した深度データをそのImage型にコピーすればOKです。（glReadPixelsのデータについて多少の加工が必要かもしれません。）このように書いてみると案外簡単に実現できそうな気もしますね。
私は当初この件よく把握できていなかったのですが、升谷先生がそこまで理解の上で修正箇所などもご検討くださっていて頭が下がります。
もしそのあたりトライしていただけるのであれば、ブランチを作って作業していただいて、プルリクなど送っていただければ、取り込むようにします。途中まで実装したけどまだうまくいかないといった状況でも、とりあえずプルリクしていただければみるようにいたします。
中岡様，
回答ありがとうございます．GLVisionSimulatorItem.cppをもう少し読んでみて，
glReadPixels(0, 0, pixelWidth, pixelHeight, GL_DEPTH_COMPONENT, GL_FLOAT, &depthBuf[0]);
を直接使うのは難しそうだというのは，わかりました．
WRSまでにBodyROSItemに反映していただくことは考えていません．現在，choreonoid_rosからフォークした内容で，あれこれいじっています．ただ，基本的な機能だと思いますので，確認を兼ねて提案してみました．
カラー画像と深度画像と別のトピックでいいと思います．RealSense用のノードではそのような仕様になっています．
BodyROSItemにコードを追加して，深度画像のパブリッシュは一応できています．ただし，効率の良いコードかどうかはわかりません．image_transportを使って圧縮もできています．具体的なコードは，このスレッドの上の方にあります．
https://discourse2.choreonoid.org/t/bodyros/429/4?u=masutani
実は，ここで言ってみても仕方がないことではあるのですが，WRSの競技は，2021年2月のWRS2020ブランチで実施するようです．masterブランチはそこからかなり変化しているようで，WRSブランチでは「ROS版Tankチュートリアル」のビルドが通りません（それに気づかずハマりました）．同じ理由で，choronoid_rosも1月20日のコミットからフォークしています．ということで，今書いているコードをそのままプルリクエストするのはできないと思います．