本記事の目的
以前以下の記事で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ファイルを作成します。
<?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の修正を行います。
<?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します。
<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ファイルは以下のリポジトリにあります。適宜参照をしてください。
参考文献