More than 3 years have passed since last update.

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

Last updated at 2021-05-19Posted at 2021-05-15

本記事の目的

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