ira_laser_toolsを使用して2つのlidarのscanデータを統合しよう！

# 本記事の目的

2つの異なるlidarのscanデータを1つのscanデータに統合する事を目的とします。

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

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

UbuntuやROSの環境構築は別途他のサイトをご覧ください。

# ira_laser_toolsを使用して2つのlidarのscanデータを統合
## 2つのrplidarを起動

まずは実行権限を与えます。

```bash
$ sudo chmod 666 /dev/ttyUSB0
```
```bash
$ sudo chmod 666 /dev/ttyUSB1
```

ここでUSB0とUSB1は差した順番に依存します。今回はfrontをfirst_lidarとしているのでfront側を先に接続し、back側を二番目に接続します。

次に2つのlidarを起動します。

```xml:rplidar.launch
<launch>
  <!-- first laser -->
  <node name="first_rplidar"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>
    <param name="frame_id"            type="string" value="front_lidar"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
    <remap from="/scan" to="/front_scan" />
  </node>

  <!-- second laser -->
  <node name="second_rplidar"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB1"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>
    <param name="frame_id"            type="string" value="back_lidar"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
    <remap from="/scan" to="/back_scan" />
  </node>
</launch>
```
ここでそれぞれのscanデータをremapしています。rempaについては以下の記事を参照してください。

https://qiita.com/strv/items/f85d216aad4799e418d0

また同一のnodeを同時に起動する事は出来ないので、ノードの名前の変更は必ずしてください。今回は**node name="first_rplidar"**,**node name="second_rplidar"**という名前を付けました。

https://robot.isc.chubu.ac.jp/?p=991

次に基準となるframeやlidarの計測範囲などを設定します。

```xml:laserscan_multi_merger.launch
<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 /back_scan" /> <!-- 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>
```

最後にfrontとbackのframeのstatic_tfを発行します

```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_back" args=" -0.2 0 0 3.14 0 0 front_lidar back_lidar 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>
```

今回のlidarの位置関係が以下の画像のようになっているため、

![two_lider](https://user-images.githubusercontent.com/52307432/118369298-0c614300-b5de-11eb-959e-0bbe969facf0.jpg)

```xml
<node pkg="tf" type="static_transform_publisher" name="front_to_back" args=" -0.2 0 0 3.14 0 0 front_lidar back_lidar 100" />
```
と発行しています。こちらの値については使用する環境に合わせて調節してください。

rpliderの座標系については以下の素晴らしい記事を参考にしてください。

https://qiita.com/motoms/items/382bfcee80aa05f5dcd9

上記記事にも書かれているように、進行方向の向きについて注意してください。

## 統合結果

以下のコマンドを順に実行します。

```bash
$ roslaunch ira_laser_tools rplidar.launch
$ roslaunch ira_laser_tools merge_lidar.launch
```

topicとしては以下のようなものが発行されています。

```xml/back_scan
/clicked_point
/front_scan
/initialpose
/laserscan_multi_merger/parameter_descriptions
/laserscan_multi_merger/parameter_updates
/merged_cloud
/move_base_simple/goal
/rosout
/rosout_agg
/scan
/tf
/tf_static
```

frontのscanデータは以下のようになります。

![Screenshot from 2021-05-15 08-03-03](https://user-images.githubusercontent.com/52307432/118369507-9a3d2e00-b5de-11eb-89f4-a4b2fc94942d.png)

backのscanデータは以下のようになります。

![Screenshot from 2021-05-15 08-02-36](https://user-images.githubusercontent.com/52307432/118369491-8b567b80-b5de-11eb-9e00-8ffa36d543d7.png)

frontとbackのscanデータを統合した結果が以下になります。

![Screenshot from 2021-05-15 08-03-30](https://user-images.githubusercontent.com/52307432/118369520-a6c18680-b5de-11eb-84ff-dea73e90d3c9.png)

2つのliderの点群を統合しているため、点群が密になっている事が分かります。

もちろん統合したscanデータを利用して地図生成を行う事も出来ます。試しにhector_slamで地図生成をしてみました。

![Screenshot from 2021-05-15 08-50-09](https://user-images.githubusercontent.com/52307432/118370086-0a4cb380-b5e1-11eb-9695-7affe2ad5db8.png)

地図生成については以下の素晴らしい記事を参照してください。

https://qiita.com/motoms/items/4c45f75911e210088ea1

上記の記事ではgmapping_slam, hector_slam, cartgrapher全ての実行手順が記載されているので、お好きなslamを試してみてください。

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

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

# まとめ
- 複数のliderを統合したことで点群を密にすることが出来ました
- 統合したscanデータを利用して地図生成を行いました

# 参考文献
http://wiki.ros.org/ira_laser_tools

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