RealSenseD435iとRPLidarのscan情報を統合しよう！

# 本記事の目的

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

https://qiita.com/MMM-lab/items/84cdf9a8ad97324e10ed

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

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

## depthimage_to_laserscan

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

https://github.com/ros-perception/depthimage_to_laserscan

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

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

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

```xml: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データと統合を行うために別の名前に設定しています。

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

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

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

![Screenshot from 2021-04-24 23-50-57](https://user-images.githubusercontent.com/52307432/115964245-7b5af700-a55e-11eb-9c21-3e2ea4e261f3.png)

# ira_laser_tools

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

https://qiita.com/MMM-lab/items/84cdf9a8ad97324e10ed

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

```xml: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します。

```xml: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の位置関係は以下の画像のようになっています。

![S__19963906](https://user-images.githubusercontent.com/52307432/118787916-3ceef880-b8ce-11eb-954d-4478c84749d8.jpg)

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

```bash
$ 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データ**

![Screenshot from 2021-05-19 01-54-41](https://user-images.githubusercontent.com/52307432/118788430-c272a880-b8ce-11eb-9c07-39b3a174176b.png)

- **cameraのscanデータ**

![Screenshot from 2021-05-19 01-54-51](https://user-images.githubusercontent.com/52307432/118788490-cf8f9780-b8ce-11eb-8f23-6a78c211b036.png)

- **統合したscanデータ**

![Screenshot from 2021-05-19 01-55-03](https://user-images.githubusercontent.com/52307432/118788541-db7b5980-b8ce-11eb-8332-c4e559ced758.png)

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

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

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

![Screenshot from 2021-05-19 01-59-14](https://user-images.githubusercontent.com/52307432/118788892-31e89800-b8cf-11eb-9f5b-74fb1d22ecb2.png)

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

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

https://github.com/TUKUBA-CHALLENGE/ira_laser_tools/tree/depth_lider_integrate

# 参考文献

https://rb-station.com/blogs/article/ros-depthimage_to_laserscan

https://rb-station.com/blogs/article/ros-laserscan_multi_merger