移動ロボットで地図を作成（slam gmapping)

移動ロボットのモデルを作成した後に２D-LRF（レーザレンジファインダ）を取り付けるとgmappingというnodeを使うと簡単に地図作成ができるようになります．

2D-LRFを前回作った移動ロボットに取り付けるところから始めます．レーザを取り付ければ後はgmappingのnodeを呼び出すlaunchファイルを書くだけで簡単に終了！．試してください．

１．2D-LRFの取り付け

前回作った移動ロボットはxacroを使って構築しましたが，そのファイルのうち「wheel_robot_base.urdf.xacro」の一番最後に形状データとgazeboに用意されているレーザのプラグイン読み込みとそのプラグインで調整可能なレーザの仕様設定をするだけとなります．

追加する具体的な記述は下記の通り．

レーザのモデルは北陽電機のURGと思われるような名前「hokuyo」が使われていますが仕様があっていませんので，合わせたいときはリンクのページの仕様に合わせてください．

gazeboのオリジナルの説明のページはここ．出力トピックを/rrbot/laser/scanから/scanにしてすぐにslamで使えるようにしています．

レーザレンジファインダの記述

<!-- Hokuyo Laser scanner -->

  <property name="hokuyo_size" value="0.05" />

  <!-- hokuyo -->

  <link name="hokuyo_link">

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="${hokuyo_size} ${hokuyo_size} ${hokuyo_size}"/>

      </geometry>

      <material name="Blue" />

    </visual>

  </link>

  <joint name="hokuyo_joint" type="fixed">

<!--    

<origin xyz="${base_radius - hokuyo_size/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />

-->

    <origin xyz="${hokuyo_size/2} 0 ${0.2+hokuyo_size/4}" rpy="0 0 0" />

    <parent link="base_link"/>

    <child link="hokuyo_link" />

  </joint>

  <gazebo reference="hokuyo_link">

    <material>Gazebo/Blue</material>

    <turnGravityOff>false</turnGravityOff>

    <sensor type="ray" name="head_hokuyo_sensor">

      <pose>${hokuyo_size/2} 0 0 0 0 0</pose>

      <visualize>false</visualize> <!--レーザ表示したいときはこの行削除-->

      <update_rate>40</update_rate>

      <ray>

        <scan>

          <horizontal>

            <samples>720</samples>

            <resolution>1</resolution>

            <min_angle>-1.570796</min_angle>

            <max_angle>1.570796</max_angle>

          </horizontal>

        </scan>

        <range>

          <min>0.10</min>

          <max>10.0</max>

          <resolution>0.001</resolution>

        </range>

      </ray>

      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">

        <topicName>/scan</topicName>

        <frameName>hokuyo_link</frameName>

      </plugin>

    </sensor>

  </gazebo>

上記の記述を追加して前回と同じくモデルを確認するために下記の（前回の）launchを実行するとLRFが付いたモデルが表示されます．

$ roslaunch wheel_robot wheel_robot.launch

レーザが移動ロボットの中心付近に浮いて取り付けてありますが，気になる人は調整してください．

下記に上記レーザの記述を追加した「wheel_robot_base.urdf.xacro」を記載しておきます．

wheel_robot_base.urdf.xacro

<?xml version="1.0"?>

<robot name="wheel_robot_base" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Included URDF/XACRO Files -->

  <xacro:include filename="$(find robot_pkg)/urdf/materials.urdf.xacro" />

  <xacro:include filename="$(find robot_pkg)/urdf/wheel.urdf.xacro" />

  <!-- PROPERTY LIST -->

  <!--All units in m-kg-s-radians unit system -->

  <property name="M_PI" value="3.1415926535897931" />

  <!-- Main Body-base -->

  <property name="base_x_size" value="1.0" /> 

  <property name="base_y_size" value="0.5" /> 

  <property name="base_z_size" value="0.25" />

  <property name="base_mass" value="35" /> <!-- in kg-->

  <!--Inertial macros for the box and cylinder. Units are kg*m^2-->

  <macro name="box_inertia" params="m x y z">

    <inertia  ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"

              iyy="${m*(x*x+z*z)/12}" iyz = "0"

              izz="${m*(x*x+z*z)/12}" /> 

  </macro>

  <!-- BASE-FOOTPRINT -->

  <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->

  <link name="base_footprint">

    <inertial>

      <mass value="0.0001" />

      <origin xyz="0 0 0" />

      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"

          iyy="0.0001" iyz="0.0" 

          izz="0.0001" />

    </inertial>

    <visual>

        <origin xyz="0 0 0" rpy="0 0 0" />

        <geometry>

            <box size="0.001 0.001 0.001" />

        </geometry>

    </visual>

  </link>

  <gazebo reference="base_footprint">

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

  <joint name="base_footprint_joint" type="fixed">

  <origin xyz="0 0 ${wheel_radius - base_z_origin_to_wheel_origin}" rpy="0 0 0" />

    <parent link="base_footprint"/>

    <child link="base_link" />

  </joint>

  <!-- BASE-LINK -->

  <!--Actual body/chassis of the robot-->

  <link name="base_link">

    <inertial>

      <mass value="${base_mass}" />

      <origin xyz="0 0 0" />

      <!--The 3x3 rotational inertia matrix. -->

      <box_inertia  m="${base_mass}" x="${base_x_size}" y="${base_y_size}" z="${base_z_size}"/> 

    </inertial>    

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="1 0.5 0.25"/>

      </geometry>

      <material name="Yellow" />

    </visual>  

    <collision>

      <origin xyz="0 0 0" rpy="0 0 0 " />

      <geometry>

        <box size="1 0.5 0.25"/>

      </geometry>

    </collision>     

  </link>

  <gazebo reference="base_link">

    <material>Gazebo/Yellow</material>

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

  <!-- WHEELs -->

  <wheel fb="front" lr="right" parent="base_link" translateX="1" translateY="-1" flipY="-1"/>

  <wheel fb="front" lr="left" parent="base_link" translateX="1" translateY="1" flipY="-1"/>

  <wheel fb="back" lr="right" parent="base_link" translateX="-1" translateY="-1" flipY="-1"/>

  <wheel fb="back" lr="left" parent="base_link" translateX="-1" translateY="1" flipY="-1"/>

  <gazebo>

    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">

      <alwaysOn>true</alwaysOn>

      <updateRate>100.0</updateRate>

      <leftFrontJoint>front_left_wheel_joint</leftFrontJoint>

      <rightFrontJoint>front_right_wheel_joint</rightFrontJoint>

      <leftRearJoint>back_left_wheel_joint</leftRearJoint>

      <rightRearJoint>back_right_wheel_joint</rightRearJoint>

      <wheelSeparation>${base_y_size}</wheelSeparation>

      <wheelDiameter>${2*wheel_radius}</wheelDiameter>

      <torque>35</torque>

      <broadcastTF>1</broadcastTF>

      <odometryFrame>map</odometryFrame>

      <commandTopic>cmd_vel</commandTopic>

      <odometryTopic>odom</odometryTopic>

      <robotBaseFrame>base_footprint</robotBaseFrame>

    </plugin>

    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />

  </gazebo>

  <!-- Hokuyo Laser scanner -->

  <property name="hokuyo_size" value="0.05" />

  <!-- hokuyo -->

  <link name="hokuyo_link">

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="${hokuyo_size} ${hokuyo_size} ${hokuyo_size}"/>

      </geometry>

      <material name="Blue" />

    </visual>

  </link>

  <joint name="hokuyo_joint" type="fixed">

<!--    

<origin xyz="${base_radius - hokuyo_size/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />

-->

    <origin xyz="${hokuyo_size/2} 0 ${0.2+hokuyo_size/4}" rpy="0 0 0" />

    <parent link="base_link"/>

    <child link="hokuyo_link" />

  </joint>

  <gazebo reference="hokuyo_link">

    <material>Gazebo/Blue</material>

    <turnGravityOff>false</turnGravityOff>

    <sensor type="ray" name="head_hokuyo_sensor">

      <pose>${hokuyo_size/2} 0 0 0 0 0</pose>

      <visualize>false</visualize>

      <update_rate>40</update_rate>

      <ray>

        <scan>

          <horizontal>

            <samples>1024</samples>

            <resolution>1</resolution>

            <min_angle>-1.570796</min_angle>

            <max_angle>1.570796</max_angle>

          </horizontal>

        </scan>

        <range>

          <min>0.10</min>

          <max>10.0</max>

          <resolution>0.001</resolution>

        </range>

      </ray>

      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">

        <topicName>/scan</topicName>

        <frameName>hokuyo_link</frameName>

      </plugin>

    </sensor>

  </gazebo>

</robot>

つぎに，gmappingで地図作成を行うlaunchファイルを記載します．

一応，移動ロボットのマッピング専用のnodeを作成しておき，そこにlaunchファイルを書くことにします．

下記コマンドでnodeとlaunchディレクトリを作成してwheel_robot_gmapping.launchを記載します．

$ cd ~/catkin_ws/src

$ catkin_create_pkg wheel_robot_gmapping std_msgs roscpp

$ cd wheel_robot_gmapping

$ mkdir launch

$ cd launch

$ gedit wheel_robot_gmapping.launch

slam_gmappingのノードはレーザのデータ「sensor_msgs./LaserScan」トピックとtfのデータを読み込んで（sabscribe)して，地図データ「nav_msgs/OccupancyGrid」を出力（publish）します．

詳しくはroswikiのhttp://wiki.ros.org/gmappingを参考にしてください．

wheel_robot_gmapping.launchは，これらの情報をgmappingに与える設定をして立ち上げるだけのlaunchファイルとなります．

wheel_robot_gmapping.launch

<launch>

  <arg name="scan_topic" default="scan" />

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <param name="base_frame" value="base_footprint"/>

    <param name="odom_frame" value="odom"/>

    <param name="map_update_interval" value="5.0"/>

    <param name="maxUrange" value="6.0"/>

    <param name="maxRange" value="8.0"/>

    <param name="sigma" value="0.05"/>

    <param name="kernelSize" value="1"/>

    <param name="lstep" value="0.05"/>

    <param name="astep" value="0.05"/>

    <param name="iterations" value="5"/>

    <param name="lsigma" value="0.075"/>

    <param name="ogain" value="3.0"/>

    <param name="lskip" value="0"/>

    <param name="minimumScore" value="100"/>

    <param name="srr" value="0.01"/>

    <param name="srt" value="0.02"/>

    <param name="str" value="0.01"/>

    <param name="stt" value="0.02"/>

    <param name="linearUpdate" value="0.5"/>

    <param name="angularUpdate" value="0.436"/>

    <param name="temporalUpdate" value="-1.0"/>

    <param name="resampleThreshold" value="0.5"/>

    <param name="particles" value="80"/>

  <!--

    <param name="xmin" value="-50.0"/>

    <param name="ymin" value="-50.0"/>

    <param name="xmax" value="50.0"/>

    <param name="ymax" value="50.0"/>

  make the starting size small for the benefit of the Android client's memory...

  -->

    <param name="xmin" value="-1.0"/>

    <param name="ymin" value="-1.0"/>

    <param name="xmax" value="1.0"/>

    <param name="ymax" value="1.0"/>

    <param name="delta" value="0.05"/>

    <param name="llsamplerange" value="0.01"/>

    <param name="llsamplestep" value="0.01"/>

    <param name="lasamplerange" value="0.005"/>

    <param name="lasamplestep" value="0.005"/>

    <remap from="scan" to="$(arg scan_topic)"/>

  </node>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />

    <rosparam file="$(find diff_mobile_robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />

    <rosparam file="$(find diff_mobile_robot)/param/local_costmap_params.yaml" command="load" />

    <rosparam file="$(find diff_mobile_robot)/param/global_costmap_params.yaml" command="load" />

    <rosparam file="$(find diff_mobile_robot)/param/base_local_planner_params.yaml" command="load" />

    <rosparam file="$(find diff_mobile_robot)/param/dwa_local_planner_params.yaml" command="load" />

    <rosparam file="$(find diff_mobile_robot)/param/move_base_params.yaml" command="load" />

<!--    <remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>

    <remap from="odom" to="$(arg odom_topic)"/> -->

  </node>

</launch>

細かいパラメータがきになるところだが，

実際に利用するときに調整するパラメータと思ったほうが良い．

各パラメータの説明は本家のページを見て感覚的に微調整していくしか無い．

何も調整しなくてもそこそこ良い地図作成をしてくれるはず．

それでは，このlaunchファイルを使って地図を作成する．

$ roslaunch wheel_robot wheel_robot.launch（１つ目のウィンドウ）

$ roslaunch wheel_robot_gmapping wheel_robot_gmapping.launch（２つ目のウィンドウ）

$ rqt （３つ目のウィンドウでRobot ToolsのRobot Stearingを使うため）

$ rosrun rviz rviz （4つ目のウィンドウでrvizで地図を表示）

１つめのlaunchファイルでgazeboまで立ち上げてくれているはずなので，gazebo内に障害物などをおいておきます．

（何もないと地図を作成できません）

これで準備が完了したのでrqtのRobot ToolsのRobot Stearingを使って移動ロボットを動かします．

rivzの画面では，rvizにマップを表示するために、rvizの設定を下記のものにする

• Displaysの「Add」で「RobotModel」を追加

• Displaysの「Global Options」の「Fixed Frame」を「base_footprint」に設定

• Displaysの「Add」で「LaserScan」を追加

• Displaysの「LaserScan」の「Topic」を「/scan」に設定

• Displaysの「Add」で「Map」を追加

• Displaysの「Map」の「Topic」を「/map」に設定

あとは，移動ロボットを動かしていけば地図が出来上がる様子を確認できます．

せっかく作った地図なので，保存したい場合は下記のコマンドで地図を保存できます．

map_sever（マップサーバ）のsaveコマンドmap_saverを使う．serverとsaverを間違えないように！

マップはどこでもセーブして良いですが，今回はwheel_robot_gmappingのディレクトリの中にmapディレクトリを作成して保存します．

$ cd ~/catkin_ws/src/wheel_robot_gmapping/

$ mkdir map

$ rosrun map_server map_saver -f test（testは自分の好きなファイル名．このコマンドを実行したディレクトリに保存される）

$ ls （lsでどんなファイルができているか確認する）

test.pgm test.yaml （この２つのファイルが有ればOK)

保存するとpgm形式の画像ファイルとマップ画像と同じ名前のyaml形式の設定ファイルが保存されます．

ナビゲーションをしたい時は画像ファイルだけでなくyamlファイルの設定ファイルも必要になります．

以上で簡単なgmappingの利用例の説明を終わります．