[ROS melodic] Intel RealSense D435iで/scanと/imuのトピックを発行する

ROS melodicにおいて、小型の深度センサーである、Intel RealSense D435iを用いて、マップ生成や自己位置推定などに必要な/scanと/imuのトピックを発行したいと思います。RealSenseにはD435とD435iと、形状が似ているタイプが存在しますが、D435iの方がIMU（ジャイロや加速度センサー）がついているため、こちらを使用します。

RealSenseライブラリのインストール

こちらのrealsense-rosがRealSenseのROSライブラリになります。

https://github.com/IntelRealSense/realsense-ros

ROS melodic向けのRealSenseライブラリは上記のREADMEを参考にしてインストールしましょう。

roslaunch realsense2_camera rs_camera.launch

インストール後、上記コマンドを実行し、別のターミナルを開いてrostopic list -vなどとコマンドを打ってtopicのリストを表示した時に、/camera/color/camera_infoなどのPublished Topicsに記載されているtopicのリストが表示されていれば無事にインストールと実行ができています。

デプス画像から/scanトピックを発行する

マップ生成をする際、/scanトピックをsubscribeする必要がありますが、realsense-rosで発行できるトピックの中に/scanトピックはありません。

そこで、デプス画像を/scanトピックに変換できる下記ライブラリを使用します。

depthimage_to_laserscan

元となるデプス画像（depthimage_to_laserscanのページより）

scanデータに変換される（depthimage_to_laserscanのページより）

sudo apt-get install ros-melodic-depthimage-to-laserscan

でインストールするか、下記のようにcatkinでインストールをします。

cd catkin_ws/src
git clone https://github.com/ros-perception/depthimage_to_laserscan.git
cd ..
catkin_make

realsense-rosでは、デプス画像は/camera/depth/image_rect_rawというトピックで発行されているので、

roslaunch realsense2_camera rs_camera.launch

rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_rect_raw

と、２つのターミナルで実行してみましょう。

rostopic list -v で確認した時に/scanが発行されていれば、無事にデプス画像からスキャンデータに変換ができていることになります。

また、launchファイルに組み込む際は、下記などのようにします。

 <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" >
     <param name="scan_height" value="3"/>
     <param name="output_frame_id" value="base_link"/>
     <remap from="image" to="/camera/depth/image_rect_raw" />
 </node>

/imuトピックを発行する

また、自己位置推定のために/imuのトピックを発行する必要がありますが、realsense-rosはデフォルトでは発行されません。

githubのレポジトリのページに説明がありますが、launchファイルを実行する際に、unite_imu_methodというパラメータにlinear_interpolationをセットしてあげる必要があります。これにより、gyroとaccelの２つのストリームを統合して/imuトピックを発行することができます。

roslaunch realsense2_camera rs_camera.launch unite_imu_method:=linear_interpolation

上記を実行した後にrostopic list -vで確認をすると/imuトピックが発行されていると思います。

また、launchファイルに組み込む時は下記などのようにします。

 <include file="$(find realsense2_camera)/launch/rs_camera.launch">
   <arg name="unite_imu_method" value="linear_interpolation"/>
 </include>

※2021年2月26日追記　　現在のrs_camera.launchはenable_gyroとenable_accelというパラメータがデフォルトでfalseになってしまったので、trueを指定する必要があります。

  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="unite_imu_method" value="linear_interpolation"/>
    <arg name="enable_gyro" value="true"/>
    <arg name="enable_accel" value="true"/>
  </include>