先端技術調査グループの大久保です。
弊社では現在、クラウド上でROSの開発が行えるAWS RoboMakerを利用しており、GazeboシミュレーションもRoboMakerを使って行っています。当ブログでも、RoboMakerを使ったシミュレーションを以前取り上げています。
現在は、シミュレーション上のロボットにdepthカメラを取り付け、depth情報を収集できるようにしています。
このdepth情報ですが、32bit浮動小数点数のバイナリ列のため、そのままでは可視化して確認することができません。ROS用のツールを使って可視化することはできますが、弊社のVisual M2Mなら、ROSトピックとして流れる画像をネットワーク越しに確認することができるため、これを利用します。その時必要になるのは、depth情報のROSトピックをjpegに変換して、それを別のROSトピックに流すノードとなります。
ROSのノードを記述するためにはC++かPythonを使うのが一般的ですが、Rustでも記述することができます。そこで今回は、Rustを使ってノードを書いてみます。
ビルドの設定
Robomaker上のプロジェクトはロボット本体へデプロイするコードを集めたworkspaceと、シミュレーションのためのworkspaceで構成されていますが、今回はロボット用のworkspaceであるrobot_wsにRust製のノードを追加します。depth情報をjpegにするため、depth2jpegという名前を付けます。次のようにROSパッケージを追加します。
cd robot_ws/src catkin_create_pkg depth2jpeg std_msgs sensor_msgs cd depth2jpeg mkdir src && cd src cargo new depth2jpeg
Robomakerを使う場合、CMakeLists.txtにcolcon buildでCargoを呼び出してビルドするよう設定する必要があります。また、colcon bundleで実行ファイル等の必要なアセットがバンドルされるよう設定する必要もあります。そのために、自動生成されたCMakeLists.txtに次の内容を追記します。設定の中にある各パスの指定はプロジェクトの構成に応じてうまく変えてやります。
# ビルド用の設定
# ビルド時にcargoが呼び出されるようにする。Cargo.tomlのパスを指定
add_custom_target(depth2jpeg
ALL
COMMAND cargo build --release --manifest-path ../../src/depth2jpeg/src/depth2jpeg/Cargo.toml
)
# launchディレクトリをバンドルするための設定
# launchディレクトリにはlaunchファイルを入れる
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
# cargoのビルドで生成した実行ファイルをバンドルするための設定
install(PROGRAMS src/depth2jpeg/target/release/depth2jpeg
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
RustでROSのノードを記述するには、rosrustを用います。その他の必要になるクレートを含めると、Cargo.tomlには以下のように依存関係を記述しておきます。
[dependencies] rosrust = "0.9" rosrust_msg = "0.1" image = "0.23" byteorder = "1"
前述の通りrosrustを入れておきます。rosrust_msgは、入れておくことでビルド時にその環境で定義されているROSメッセージを自動でインポートしてくれる便利なクレートです。imageはJPEGへのエンコード用、byteorderはバイナリ形式のdepthを読むのに使います。
Rustでノードを書いていく
depth情報が入っている/depthcam/depth/image_rawという名前のトピックを受け取り、それをjpegに変換したら/depthcam/depth/jpegというトピックで送出するもととします。この場合、main関数は以下のように記述します。
use byteorder::{ByteOrder, LittleEndian}; use rosrust; use rosrust_msg::sensor_msgs::{CompressedImage, Image}; use std::sync::Arc; fn main() { // depth2jpegという名前でノードを初期化 rosrust::init("depth2jpeg"); // 送出用のトピックを開く。Arcで囲む let p = Arc::new(rosrust::publish("/depthcam/depth/jpeg", 2).unwrap()); // 受信時用のコールバックを登録する let _subscriber_info = rosrust::subscribe("/depthcam/depth/image_raw", 2, move |img: Image| { p.send(depth2jpeg(img)).unwrap(); }) .unwrap(); // 終了のシグナルを受信するまで待つ rosrust::spin(); }
やっていることは単純で、トピックを2つ開き、コールバックを登録するだけです。受け取るトピックの型にはImage、送出するトピックの型はCompressedImageです。この変換を行うのはdepth2jpeg関数です。この関数は以下のように記述します。
fn depth2jpeg(img: Image) -> CompressedImage { let width = img.width; let height = img.height; let raw_data = &img.data; // 32FC1 littleendianをVecへデコード let mut i = 0usize; let pixels: Vec<f32> = std::iter::from_fn(move || { if i < (width * height) as usize { let value = LittleEndian::read_f32(&raw_data[(i * 4)..(i * 4 + 4)]); i += 1; Some(value) } else { None } }) .collect(); // ピクセルのうち、深度が最大のものを探す let mut max = 0.0; for pixel in &pixels { if *pixel > max { max = *pixel; } } // RGB24bitのバイト列への変換 let pixels: Vec<u8> = pixels .iter() .flat_map(|pixel| { if !pixel.is_nan() { // 最大深度のところを白として深度をグレースケールに変換する let d = (pixel * 255.0 / max) as u8; vec![d, d, d] } else { // 深度がNaNのときは赤にする vec![255, 0, 0] } }) .collect(); // jpegにエンコード let mut data = Vec::new(); let mut encoder = image::jpeg::JPEGEncoder::new(&mut data); encoder .encode(&pixels, width, height, image::ColorType::Rgb8) .unwrap(); // CompressedImageを作成 CompressedImage { header: img.header, format: "rgb8; jpeg compressed bgr8".to_owned(), data: data, } }
Image.dataはVec<u8>なので、これをエンディアンに注意しつつVec<f32>に変換してやります。その後深度の最大値を求め、最大深度のピクセルを白としてグレースケールに変換してやります。Gazeboによるdepthカメラは、ある程度の近距離、もしくは遠距離になると、NaNが格納されるため、そこは赤色とします。
実行
CMakeLists.txtと同じディレクトリ内にlaunchディレクトリを作成し、その下にdepth2jpeg.launchを作成します。ただ実行ファイルを呼び出すのだけですので以下のようにします。
<launch> <node pkg="depth2jpeg" name="depth2jpeg" type="depth2jpeg" /> </launch>
あとはこのlaunchファイルが、robomakerのジョブが立ち上がった時に参照されるように記述してやれば、このノードが立ち上がります。この設定はプロジェクトごとに異なるので割愛します。
jpegで流れているトピックがVisual M2Mで見られるよう設定し、見てみた結果が以下のようになります。
画面のほとんどが赤くて測定範囲外であることがわかりますが、それ以外の部分では深度情報が拾えていることがわかります。
Rustを使ってみた感想
Rustを使うとPythonに比べるとバイナリ操作が普通に書けるのがありがたいです。また、RoboMakerを使って開発していると、「修正→ビルド→バンドル→S3にアップロード→シミュレーションジョブの立ち上げ」という一連のプロセスにそれなりの時間がかかります。Pythonだと実行してみるまでエラーが分からないので、コンパイル時にエラーが検出できるのはかなり大きいです。C++でもいいのですが、イテレータ等で洗練した感じに書けたり、jpegエンコーダのようなライブラリもCargo.tomlに設定書くだけで簡単に使えるのはかなり楽です。
こうして見ると、ロボティクス分野でRustを使うポテンシャルは結構大きいのでは、と考えています。そのため、今後もロボティクス分野でのRustの可能性を探っていこうと思います。
