laser_scan_matcherを用いてnavigationをしよう！

本記事の目的

laser_scan_matcherを使用してscanとimuデータからodomを取得し、navigation stackを用いてシンプルなnavigationを実行します。

環境

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

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

laser_scan_matcherのインストール

bainaryからインストールする場合は以下のコマンドを実行してください。

$ sudo apt-get install python-wstool
$ sudo apt-get install ros-melodic-scan-tools

source codeからインストールする場合は以下のコマンドを実行してください。

$ cd ~/catkin_ws/src
$ git clone https://github.com/AndreaCensi/csm
$ cd ../
$ sudo apt install libgsl-dev
$ catkin_make_isolated
$ cd ~/catkin_ws/src
$ git clone https://github.com/ccny-ros-pkg/scan_tools.git
$ cd ../
$ catkin_make_isolated
$ source devel/setup.bash

IMU Magcwick filter

RealSense D435iはIMUが搭載されているので、それらの値を使用してMadgwick Filterでカメラの姿勢を算出します。Madgwick Filterはクォータニオンを軸に姿勢推定を行います。Kalman filterとは異なり行列演算が少なく計算コストが少ない事で有名です。クォータニオン及びMadgwick Filterについては以下のサイトで非常に良くまとめられているので適宜参照してください。

それでは以下のコマンドを順に実行してimu_toolsをインストールしましょう。

$ git clone -b melodic https://github.com/ccny-ros-pkg/imu_tools.git
$ rosdep install imu_tools
$ cd ~/catkin_ws
$ catkin_make

動作確認

• ターミナル1

$ roslaunch realsense2_camera rs_camera.launch align_depth:=true unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true

• ターミナル2

$ rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" imu/data_raw:=/camera/imu imu/data:=/rtabmap/imu

以下のような通知が出てくればOK!

[ INFO] [1621263499.479304076]: Starting ImuFilter
[ INFO] [1621263499.489995763]: Using dt computed from message headers
[ INFO] [1621263499.490097956]: The gravity vector is kept in the IMU message.
[ INFO] [1621263499.515613935]: Imu filter gain set to 0.100000
[ INFO] [1621263499.515744671]: Gyro drift bias set to 0.000000
[ INFO] [1621263499.515837592]: Magnetometer bias values: 0.000000 0.000000 0.000000
[ INFO] [1621263499.734541161]: First IMU message received.

実際にMagcwick filterで計算された姿勢の値を見てみましょう

$ rostopic echo /imu/rpy/filtered

単位はradで出てきています。精度としてはかなりよさげなので、他でも十分使えそうです！

---
header: 
  seq: 124901
  stamp: 
    secs: 1621263825
    nsecs: 744139671
  frame_id: "camera_imu_optical_frame"
vector: 
  x: -1.47213753329
  y: 0.0236475542387
  z: 0.436455833789
---
header: 
  seq: 124902
  stamp: 
    secs: 1621263825
    nsecs: 746649504
  frame_id: "camera_imu_optical_frame"
vector: 
  x: -1.47208974526
  y: 0.023968820187
  z: 0.436465254426
---

launchファイルの設定

次にnavigationを行うためのlaunchファイルを設定していきます。必要なlaunchファイルはlaser_scan_matcherに対するlaunchファイル、その他navigation stack用のamcl、move_base、それら全てを順に実行するlaunchファイルです。各種launchファイルは以下のように設定しました。

laser_scan_matcher.launch

<launch>
  <arg name="publish_covariance" default="false"/>
  
  <!-- publish an example base_link -> laser transform -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />
  <node pkg="tf" type="static_transform_publisher" name="laser_to_camera"
    args="0.0 0.0 0.0 0.0 0.0 0.0 /laser /camera_link 40" />

  <!-- start the laser scan_matcher -->
  <group if="$(arg publish_covariance)">
    <param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
    <param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
  </group>

  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <!-- Topics -->
    <remap from="scan" to="scan"/>
    <!--<remap from="cloud" to="/camera/depth/camera_info"/>-->
    <remap from="imu/data" to="/rtabmap/imu"/>
    <remap from="odom" to="odom"/>
    <remap from="vel" to="/mobile_base_controller/cmd_vel" />

    <!-- Coordinate frames -->
    <param name="fixed_frame" value="odom" />
    <param name="base_frame" value="base_link" />

    <!-- Motion prediction -->
    <param name="use_imu" value="true"/>
    <param name="use_odom" value="false"/>
    <param name="use_vel" value="true"/>

    <!-- Point cloud input -->
    <param name="use_cloud_input" value="false"/>
    <param name="cloud_range_min" value="0.4"/>
    <param name="cloud_range_max" value="10.0"/>

    <!-- Key frames -->
    <param name="kf_dist_linear" value="0.1"/>
    <param name="kf_dist_angular" value="0.175"/>

    <!-- Output -->
    <param name="publish_tf" value="true"/>
    <param name="publish_odom" value="true"/>
    <param name="publish_pose" value="true"/>
    <param name="publish_pose_stamped" value="false"/>

    <!-- Scan matching -->
    <param name="max_iterations" value="50"/>
    <param name="max_correspondence_dist" value="0.3"/>
    <param name="max_angular_correction_deg" value="45.0"/>
    <param name="max_linear_correction" value="0.5"/>
    <param name="epsilon_xy" value="0.000001"/>
    <param name="epsilon_theta" value="0.000001"/>
    <param name="outliers_maxPerc" value="0.90"/>
  </node>
</launch>

amcl.launch

<?xml version="1.0"?>
<launch>
	<arg name="initial_pose_x" default="0.0"/>
    <arg name="initial_pose_y" default="0.0"/>
    <arg name="initial_pose_a" default="0.0"/>

    <arg name="map_file" default="$(find laser_scan_matcher_navigation)/maps/room1.yaml"/>
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" output="screen"/>
    
	<node pkg="amcl" type="amcl" name="amcl">
        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
		<param name="tf_broadcast" value="true" />
		<param name="base_frame_id" value="base_link" />
		<param name="global_frame_id" value="map" />
		<param name="odom_frame_id" value="odom" />
	    <param name="odom_model_type" value="diff" />
	    <param name="transform_tolerance" value="0.2" />
	    <param name="gui_publish_rate" value="-1.0"/>
	    <param name="laser_max_beams" value="30"/>
	    <param name="laser_max_range" value="29.5"/>
	    <param name="min_particles" value="100"/>
	    <param name="max_particles" value="3000"/>
	    <param name="update_min_d" value="0.2"/>
	    <param name="kld_err" value="0.01"/>
	    <param name="kld_z" value="0.99"/>
	    <param name="odom_alpha1" value="0.2"/>
	    <param name="odom_alpha2" value="0.2"/>
	    <param name="odom_alpha3" value="0.2"/>
	    <param name="odom_alpha4" value="0.2"/>
	    <param name="odom_alpha5" value="0.2"/>
	    <param name="laser_z_hit" value="0.95"/>
	    <param name="laser_z_short" value="0.1"/>
	    <param name="laser_z_max" value="0.05"/>
	    <param name="laser_z_rand" value="0.05"/>
	    <param name="laser_sigma_hit" value="0.2"/>
	    <param name="laser_lambda_short" value="0.1"/>
	    <param name="laser_model_type" value="likelihood_field"/>
	    <param name="laser_likelihood_max_dist" value="2.0"/>
	    <param name="update_min_a" value="0.5"/>
	    <param name="resample_interval" value="2"/>
	    <param name="transform_tolerance" value="0.1"/>
	    <param name="recovery_alpha_slow" value="0.0"/>
	    <param name="recovery_alpha_fast" value="0.0"/>  
    </node>
</launch>

move_base.launch

<launch>
    <!-- move base -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <param name="controller_frequency" value="10.0" />
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
        <rosparam file="$(find laser_scan_matcher_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap"/>
        <rosparam file="$(find laser_scan_matcher_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap"/>
        <rosparam file="$(find laser_scan_matcher_navigation)/param/local_costmap_params.yaml" command="load"/>
        <rosparam file="$(find laser_scan_matcher_navigation)/param/global_costmap_params.yaml" command="load"/>
        <rosparam file="$(find laser_scan_matcher_navigation)/param/dwa_local_planner_params.yaml" command="load" />
        <rosparam file="$(find laser_scan_matcher_navigation)/param/move_base_params.yaml" command="load"/>
    </node>
</launch>

navigation.launch

<launch>
  <!-- laser scan matcher -->
  <include file="$(find laser_scan_matcher_navigation)/launch/laser_scan_matcher.launch" />
  <!-- amcl -->
  <include file="$(find laser_scan_matcher_navigation)/launch/amcl.launch" />
  <!-- move_base -->
  <include file="$(find laser_scan_matcher_navigation)/launch/move_base.launch" />
  <!-- rviz -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find laser_scan_matcher_navigation)/rviz/laser_scan_matcher.rviz"/>
</launch>

各種パラメータの意味については以下のwikiを参照してください。

またnavigation stackについては@MoriKenさんの以下の記事や、千葉工大の原先生の資料が非常に役立ちました。

実行手順

以下のコマンドを順に実行して動作確認を行います。

$ sudo chmod 666 /dev/ttyACM0
$ sudo chmod 666 /dev/ttyUSB0
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
$ roslaunch realsense2_camera rs_camera.launch align_depth:=true unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true
$ rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _publish_tf:=false _world_frame:="enu" imu/data_raw:=/camera/imu imu/data:=/rtabmap/imu
$ roslaunch rplidar_ros rplidar.launch 
$ roslaunch laser_scan_matcher_navigation navigation.launch

ROS側からcmd_velをpublishしてArduino側でsubscribeし、PWM値をモータに加えるcodeは以下の記事を参考にしてください。

navigation結果

以下のように簡易的なnavigationが出来れば成功です！

Actual movement

Rviz movement

まとめ

• laser_scan_matcherをベースにnavigationを実行しました

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

参考文献