本記事の目的

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

環境

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

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

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

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

2つのrplidarを起動

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

$ sudo chmod 666 /dev/ttyUSB0

$ sudo chmod 666 /dev/ttyUSB1

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

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

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については以下の記事を参照してください。

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

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

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を発行します

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の位置関係が以下の画像のようになっているため、

<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の座標系については以下の素晴らしい記事を参考にしてください。

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

統合結果

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

$ roslaunch ira_laser_tools rplidar.launch
$ roslaunch ira_laser_tools merge_lidar.launch

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

/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データは以下のようになります。

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

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

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

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

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

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

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

まとめ

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

参考文献