Edited at

ROS講座93 laser_filterを使う


環境

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

項目

CPU
Core i5-8250U

Ubuntu
16.04

ROS
Kinetic

Gazebo
7.14.0

インストールについてはROS講座02 インストールを参照してください。

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


概要

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


ソースコード


urdf(Xacro)

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


設定ファイル

Lasefilterの設定ファイルです。

1つ目は角度でLaserを区切るAngularBoundsFilterです。Gazeboのセンサーは-3.1415~3.1415の範囲でscanトピックを出します。-3.15~-1.57の範囲を除外するフィルターと1.57~3.15の範囲を除外するフィルターを重ねることで-1.57~1.57の前半分だけのデータにフィルタリングします。


laser_lecture/config/laser_filter1.yaml

scan_filter_chain:

- name: angle1
type: LaserScanAngularBoundsFilter
params:
lower_angle: -3.15
upper_angle: -1.57
- name: angle2
type: LaserScanAngularBoundsFilter
params:
lower_angle: 1.57
upper_angle: 3.15

2つ目は範囲で区切るBoxFilterです。これはとくていのtfから特定の範囲のscanデータを除外します。今回はロボットのサイズそのままで除外しています。

除外範囲を余裕をもって広くすると、ロボットにぶつかっている障害物が見えなくなるので注意です。


laser_lecture/config/laser_filter2.yaml

scan_filter_chain:

- name: box
type: 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 laser_lecture)/xacro/laser_box_nub.xacro" />
<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="$(find sim3_lecture)/worlds/world_test5.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>

<!-- gazebo model -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model dtw_robot" />
<node pkg="tf" type="static_transform_publisher" name="tf_1" args="0 0 0 0 0 0 map base_link 10" />
<node pkg="tf" type="static_transform_publisher" name="tf_2" args="0 0 0 0 0 0 base_link front_laser_link 10" />

<!-- 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>

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


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


実行


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講座の目次へのリンク