本記事の目的

以前以下の記事で2つのLiderのscanデータの統合を行いましたが、今回はdepthimage_to_laserscanを使用してD435iのscanデータも統合しようと思います。

環境

本記事は以下の環境で実験しています。

項目	バージョン
Ubuntu	18.04
ROS	Melodic
RPLidar	A1 M8
Realsense	D435i

depthimage_to_laserscan

深度マップのトピックをscanトピックに変換してくれるdepthimage_to_laserscan というパッケージを用いて、D435iのscanデータを取得します。

以下のコマンドを実行してインストールを行います。

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

次に以下のようなlaunchファイルを作成します。

depth_to_scan.launch

<?xml version="1.0"?>

<launch>  

  <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan_left" >
    <param name="output_frame_id" value="camera_link"/>
    <remap from="image" to="/camera/depth/image_rect_raw" />
    <remap from="camera_info" to="/camera/depth/camera_info" />
    <remap from="scan" to="/scan_camera" />
  </node>

</launch>

ここではカメラで取得する/scanを/scan_cameraとremapしています。これは後でliderのscanデータと統合を行うために別の名前に設定しています。

以下のコマンドを実行して動作確認を行います。

$ roslaunch realsense2_camera rs_camera.launch align_depth:=true
$ roslaunch ira_laser_tools depth_to_scan.launch

以下の画像のようにD435iからの深度情報をscanデータに変換する事が出来れば成功です！

ira_laser_tools

laserscan_multi_mergerは、ira_laser_toolsというパッケージに含まれる、複数のスキャントピックをマージしてくれるノードです。詳細については以下の記事を参照してください。

まず以下の様にlaserscan_multi_merger.launchの修正を行います。

laserscan_multi_merger.launch

<?xml version="1.0"?>

<launch>
        <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger" output="screen">
        	<!--<param name="destination_frame" value="cart_frame"/>-->
                <param name="destination_frame" value="front_lidar"/>
		<param name="cloud_destination_topic" value="/merged_cloud"/>
		<param name="scan_destination_topic" value="/scan"/>
                <param name="laserscan_topics" value ="/front_scan /scan_camera" /> <!-- LIST OF THE LASER SCAN TOPICS TO SUBSCRIBE -->

                <param name="angle_min" value="-3.14159"/>
                <param name="angle_max" value="3.14159"/>
                <param name="angle_increment" value="0.0058"/>
                <param name="scan_time" value="0.0333333"/>
                <param name="range_min" value="0.05"/>
                <param name="range_max" value="50.0"/>

	</node>
</launch>

注意すべきポイントはlaserscan_topics というパラメータで、今回のようにマージしたいスキャントピックが/front_scan /back_scan /scan_cameraだった場合に、 **"/front_scan /back_scan /scan_camera"**というように半角スペースで繋げます。

また今回destination_frameは前方に取り付けたliderのframe"front_lidar"に設定しています。

最後に統合するセンサ間の座標変換をpublishします。

merge_lidar.launch

<launch>
  <!-- <static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms> 
  Publish a static coordinate transform to tf using an x/y/z offset and yaw/pitch/roll. 
  The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value. -->
  <node pkg="tf" type="static_transform_publisher" name="front_to_camera" args=" 0 0 0.05 0 0 0 front_lidar camera_link 100" />
  
  <include file="$(find ira_laser_tools)/launch/laserscan_multi_merger.launch" ></include>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ira_laser_tools)/rviz/merge_lidar.rviz" />
</launch>

"static_transform_publisher"に指定するnameのパラメータは使用する環境に応じて設定し直す必要があります。

今回のlidarとcameraの位置関係は以下の画像のようになっています。

動作確認

以下のコマンドを実行して動作確認を行います。

$ roslaunch realsense2_camera rs_camera.launch align_depth:=true
$ roslaunch ira_laser_tools depth_to_scan.launch
$ roslaunch ira_laser_tools rplidar.launch
$ roslaunch ira_laser_tools merge_lidar.launch

frontのscanデータ

cameraのscanデータ

統合したscanデータ

ただ前回の360度liderに比べると、点群が密になる箇所が限られています。

統合したscanデータによる地図生成

今回も統合したscanデータで地図生成を行ってみましたが、問題なく生成できました。

まとめ

深度情報をscanデータに変換するdepthimage_to_laserscanでカメラのscanデータを取得しました
liderのscanデータと統合して地図生成を行いました

また今回使用したlaunchファイルやrvizファイルは以下のリポジトリにあります。適宜参照をしてください。

参考文献