このパッケージは、realsense2でTopic配信されるカメラパラメータ、カラー画像、カラーPCLデータを共有メモリに保存するためのnodeletです。

realsense2のノードでキャプチャされるデータを使って色々な処理をするノードを実装する場合には、Topic通信で配信される仕組みを使う方法が一般的ですが、一つのシステムで複数のノードで利用する場合には、カラー画像やPCLデータのTopic通信によるオーバーヘッドが多くなってしまいます。

そこで、realsense2のノードをなるべく変更しないように、Topic通信で配信されるカメラパラメータ、カラー画像、PCLデータを共有メモリ内に保存するような機能をもつnodeletを作成することにしました。D415を使った場合には、PCLデータのメッセージは10Mくらいになりますので、PCLデータを使うノードを複数利用する場合にはある程度の効果が期待できると思います。

shm_image_saveのリポジトリは、githubに公開しています。

https://github.com/haraisao/shm_image_save

shm_image_saveのファイル構成は下のようになってます。

shm_image_save - REAEME.md
                 - LICENSE
                 - CMakeLists.txt
                 - nodelet_descriotion.xml
                 - packacge.xml
                 - launch - realsense.launch　           # D415用ノード起動用ファイル
                 - include - shm_image_save - shmem.h
                                               - plugin_nodelet_shm_saver.h
                                               - cam_shm.h
                 - src - shmem.cpp                        # 共有メモリ用関数
                        - plugin_nodelet_shm_saver.cpp　# nodelet本体
                        - cam_shm.cpp　                   # 共有メモリアクセス用クラス
                 - scripts - shm.py　                     # 共有メモリアクセス用基底クラス
                            - img.py                       # shm_image_save の共有メモリアクセスサンプル

nodelet用のファイルは、shmem.cpp と plugin_nodelet_shm_saver.cppがあればよいのですが、このnodeletで保存したカメラパラメータなどに簡単にアクセスできるように、cam_shm.cppとshm.py, img.pyを含めています。

shm_image_saveは、ソースコードがビルド＆インストールする必要があります。このパッケージは、catkinビルドシステムに対応していると思いますが、cmakeコマンドを直接入力することでもビルドとインストールをすることができます。

パッケージをインストールするディレクトリを ~/ros とすると、下のようなコマンドでビルド可能です。

 # mkdir build
 # cd build
 # source /opt/ros/melodic/setup.bash
 # cmake -DCMAKE_INSTALL_PREFIX=~/ros ..
 # make

また、インストールする場合には、

 # make install

を実行すると完了です。インストールされるファイルは下のようになっています。

  ~/ros/include/shm_image_save/[shmem, plugin_nodelet_shm_saver, cam_shm].h
  ~/ros/lib/libshm_image_save.so
　~/ros/lib/shm_image_save/[img,shm].py
  ~/ros/lib/pkgconfig/shm_image_save.pc
  ~/ros/share/shm_image_save/[nodelet_description,package].xml
  ~/ros/share/shm_image_save/cmake/[shm_image_saveConfig-version,shm_image_saveConfig].cmake
  ~/ros/share/shm_image_save/launch/realsense.launch

shm_image_saveのインストールが終了すれば、nodeletの動作チェックを行います。このパッケージでインストールされるlaunchファイルは、ReadlSense D415で動作用です。他のデバイスで使用する場合には、realsense.launchの内容を見て、適当に修正してください。

パソコンにRealSense D415 を接続し、下のコマンドを実行します。

 # source ~/ros/setup.bash
 # roslaunch shm_image_save realsense.launch

また、別のターミナルと起動し、下のコマンドを実行します。

 # source ~/ros/setup.bash
 # rosrun shm_image_save img.py

上記の実行で、D415が撮像しているイメージが表示されれば、問題ありません。

shm_image_saveでは、CameraInfo, Image, PointCloud2の2つのデータを共有メモリにコピーするように実装しています。通常は、これらのデータ構造のままコピーした方が再利用は簡単なのですが、より汎用性を持たせるためにTopicで配信されたメッセージをシリアライズした情報をコピーするようにしています。これは、各データ構造の内部変数の型に依存せずデータとしてはuint8_tの配列として取り扱うことが可能になるという理由からです。

共有メモリの内部構造としては、下のような構造体（shmem.h内で定義）でアクセスすることを前提として領域確保をしています。

 struct cam_shm_data{
     u_int32_t max_count;
     u_int32_t cam_info_size;
     u_int32_t cam_image_size;
     u_int32_t pcl_size;
     u_int32_t cam_info_offset;
     u_int32_t cam_image_offset;
     u_int32_t pcl_offset;
     u_int32_t cam_info_count;
     u_int32_t cam_image_count;
     u_int32_t pcl_count;
     unsigned char data[];
 }

また、共有メモリへのデータコピー中に、他のノードからアクセスが起きないように、共有メモリには2つのメッセージを同時に保持するようにしています。そのため、shm_image_saveで必要となる共有メモリのデータ領域は、2MByte強になっていましたのでデフォルト値として3MByteを確保するようにしています。

上記の共有メモリの構造からもわかるとは思いますが、このnodeletでは3種類のメッセージを複数、共有メモリへコピーしています。したがって、各メッセージへのアクセスする場合の先頭ポインタを得るために、dataからのオフセットを cam_info_offset, cam_image_offset, pcl_offset に書き出しています。

つまり、共有メモリの先頭から 40 + cam_info_offset の部分に最新の CameraInfoのメッセージが保存され、その長さは cam_info_size となっています。さらにC++のプログラムでこの情報を利用する場合には、メッセージをdeserializeしなければいけませんので、実際に使う場合には、追加コードが大幅に増える可能性があります。そこで、既存のTopic通信で行っていた部分を簡単に変更するために shm_image_save::ImageSharedMemというクラスを実装しています。

このshm_image_save::ImageSharedMemには、外部からアクセス可能な内部関数として、get_cam_info(), get_cam_image(), get_pcl_data()が定義されており、それぞれ最新のカメラパラメータ、カラー画像、PCLデータを取得することが可能です。

したがって、従来PCLデータを取得、利用するために下のようなコードがあった場合、

  MyClass::MyClass()
  {
....
　   rs_cloud_sub_ = nh_.subscribe("/camera/depth_registered/points", 10,
                     &MyClass::callbackCloud,this);
....
  }

  MyClass::callbackCloud(const sensor_msgs::PointCloud2ConstPtr &msg)
  {
    subscried_cloud_ = msg;
    return;
  }

  MyClass::filterPcl()
  {
....
     PointCloud_XYZRGB_ptr cloud(new PointCloud_XYZRGB);
     pcl::fromROSMsg(*subscribed_cloud_, *cloud);
....
  }

shm_image_save::ImageSharedMemを使うと下のように書くことができます。

　#include <shm_image_save/cam_shm.h>

  MyClass::MyClass()
  {
....
　   m_imgshm_ = shm_image_save::ImageSharedMem();
....
  }

  MyClass::filterPcl()
  {
....
     subscribed_cloud_ = m_imgshm_.get_pcl_data();

     PointCloud_XYZRGB_ptr cloud(new PointCloud_XYZRGB);
     pcl::fromROSMsg(*subscribed_cloud_, *cloud);
....
  }

カメラパラメータやカラー画像を取得する場合も、実際に処理を行う前にget_cam_info(), get_cam_image()をよびだして、最新のメッセージを取得することができます。

ただし、現在の実装では、画像やPCLの更新時のイベントを発生していませんので、継続的な処理には、定期的に取得する必要があります。リクエストがあれば、更新イベントの実装は可能です。

Pythonのノードからshm_image_saveの共有メモリ領域にアクセスするためには、カメラパラメータ、カラー画像、PCLデータへのオフセットを計算する必要があります。

前述した構造体の場合には、オフセットは下のように定義することができます。

 shm_offset={'max_count':0, 'cam_info_size': 4, 'cam_image_size': 8,
     'pcl_size': 12, 'cam_info_offset': 16, 'cam_image_offset': 20,
     'pcl_offset': 24, 'cam_info_count': 28, 'cam_image_count': 32,
     'pcl_count': 36, 'data': 40}

この構造を定義を利用したアプリケーションのファイルは、scripts/shm.py, scripts.img.py にありますので、ソースコードを理解した上お使いください。img.pyに定義した ImageShmというクラスを利用すると比較的簡単に使うことができると思います。