ROS講座93 laser_filterを使う

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

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

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

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

# ソースコード
## urdf(Xacro)

下の写真のようにグレーの本体に赤いLIDARとグレーの突起のがついているロボットを作成します。
![laser_nub.png](https://qiita-image-store.s3.amazonaws.com/0/254442/9ebec82d-27e3-ed5d-cece-9f4072353c83.png)

## 設定ファイル
Lasefilterの設定ファイルです。

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

```yaml: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データを除外します。
除外範囲を余裕をもって広くすると、ロボットにぶつかっている障害物が見えなくなるので注意です。

```yaml: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

```xml: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フィルター

```shell:angleフィルター
roslaunch laser_lecture laser_filter.launch laser_filter_file:=laser_filter1.yaml
```

gazeboでは以下のように横と後ろに立方体が出現、前にはコーラ缶がランダムに出現します。
![laser_gazebo.png](https://qiita-image-store.s3.amazonaws.com/0/254442/150432e4-016c-e839-8d58-379e86230fa2.png)

緑の点がオリジナルのscanデータで、フィルター後は赤で表示されています。フィルター後は前方180度だけになっています。
![laser_filter1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/628ee600-d406-8edb-333f-66df98016dbc.png)

## boxフィルター

```shell:boxフィルター
roslaunch laser_lecture laser_filter.launch laser_filter_file:=laser_filter2.yaml
```

原点近くにあったロボットの突起の部分だけがフィルタリングされています。
![laser_filter2.png](https://qiita-image-store.s3.amazonaws.com/0/254442/33004f12-701b-36dd-74f4-e31381f198a2.png)

# 参考
[laser_filter](http://wiki.ros.org/laser_filters)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)