先端技術調査グループの大久保です。
弊社では現在、クラウド上で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の可能性を探っていこうと思います。