環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	Noetic
Gazebo	11.9.0

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

2D lidarはセンサーを360度回転させて全周囲を計測するセンサーです。実際のロボットで使っている時には、360度の視界のうちの一部はロボット自身に当たっているために取り除きたいことがあります。このような場合にはlaser_filterというパッケージを使いセンサーから出てくるscanトピックのデータをフィルタリングします。

ソースコード

urdf(Xacro)

下の写真のようにグレーの本体に赤いLIDARとグレーの突起のがついているロボットを作成します。

設定ファイル

Lasefilterの設定ファイルです。

1つ目は角度でLaserを区切るLaserScanAngularBoundsFilterです。Gazeboのセンサーは-3.1415~3.1415の範囲でscanトピックを出しますが、このフィルターで-1.57~1.57のデータだけを抜き出します。

laser_lecture/config/laser_filter1.yaml

scan_filter_chain:
- name: angle1
  type: laser_filters/LaserScanAngularBoundsFilter
  params:
    lower_angle: -1.5707
    upper_angle: 1.5707

2つ目は範囲で区切るBoxFilterです。これはx,y,zの範囲で指定したscanデータを除外します。
除外範囲を余裕をもって広くすると、ロボットにぶつかっている障害物が見えなくなるので注意です。

laser_lecture/config/laser_filter2.yaml

scan_filter_chain:
- name: box
  type: laser_filters/LaserScanBoxFilter
  params:
    box_frame: base_link
    min_x: -0.19
    max_x:  0.05
    min_y: -0.09
    max_y:  0.09
    min_z: -0.2
    max_z:  0.2

launch

laser_lecture/launch/laser_filter.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find sim_env_lecture)/urdf/sensor_station.urdf" />
  <arg name="rvizconfig" default="$(find laser_lecture)/rviz/laser_filter.rviz" />  
  <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
  <arg name="laser_filter_file" default="laser_filter1.yaml"/>  

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="sensor_station_coke_box.world"/>
  </include>

  <!-- laser_filter -->
  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
    <rosparam command="load" file="$(find laser_lecture)/config/$(arg laser_filter_file)" />
    <remap from="scan" to="/front_laser/scan" />
  </node>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />   

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

下のほうにlaser_filterを起動する部分があります。nodeの内部でconfigファイルをrosparamにloadします。

実行

各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

angleフィルター

angleフィルター

roslaunch laser_lecture laser_filter.launch laser_filter_file:=laser_filter1.yaml

gazeboでは以下のように横と後ろに立方体が出現、前にはコーラ缶がランダムに出現します。

緑の点がオリジナルのscanデータで、フィルター後は赤で表示されています。フィルター後は前方180度だけになっています。

boxフィルター

boxフィルター

roslaunch laser_lecture laser_filter.launch laser_filter_file:=laser_filter2.yaml

原点近くにあったロボットの突起の部分だけがフィルタリングされています。

参考

laser_filter

目次ページへのリンク

ROS講座の目次へのリンク