ROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る

前回までの作業でシミュレータ上で動くロボットが準備出来ましたが、今のままでは人が指示を出して動くただのラジコンでしかありません。
ここからは、障害物を避けながら目的の位置まで自立で移動するようにしていきましょう。

まずは地図を用意します。人間でも知らない場所に行く時には事前に地図を確認しますよね。ロボットにも同じように地図が必要になります。

Mapの作成

ではMapを作成していきましょう。地図の作成には、gmappingというパッケージを使用します。

http://wiki.ros.org/gmapping

このパッケージでは平面レーザーによる2D Map作成をサポートしています。これまで作成してきたロボットにGazeboシミュレータによる平面レーザーを搭載してみましょう。

平面レーザーをシミュレートする

センサーは北陽電機のものをシミュレーションしています。Gazeboのチュートリアルにこのセンサーの3D形状データが含まれており、これを使いたいいので、Gazeboのチュートリアル一式をgithubから取得します。

$ cs
$ git clone https://github.com/ros-simulation/gazebo_ros_demos.git

my_robo.urdfを編集し、ToFセンサーを削除し、平面センサーに入れ替えます。コードは次のようになります。

<?xml version="1.0"?>
<robot name="my_robo" >
  <property name="M_PI" value="3.14159274"/>

  <link name="base_link">

    <visual>
      <geometry>
        <box size="0.400 0.200 0.100"/>
      </geometry>
    </visual>

    <collision>
      <geometry>
        <box size="0.400 0.200 0.100"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0020833333" ixy="0" ixz="0" iyy="0.0070833333" iyz="0" izz="0.0083333333"/>
    </inertial>
  </link>

  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
    </inertial>

  </link>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin rpy="-1.5707 0 0" xyz="-0.100 0.130 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
    </inertial>

  </link>

  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin rpy="1.5707 0 0" xyz="-0.100 -0.130 0"/>
    <axis xyz="0 0 -1"/>
  </joint>

  <link name="caster_link">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.500"/>
      <inertia ixx="0.00025" ixy="0" ixz="0" iyy="0.00025"  iyz="0" izz="0.00025"/>
    </inertial>

  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_link"/>
    <origin xyz="0.150 0 -0.050"/>
  </joint>

  <link name="base_footprint"/>
  <joint name="base_link_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.100"/>
  </joint>

  <!-- Hokuyo Laser -->
  <link name="hokuyo_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://rrbot_description/meshes/hokuyo.dae"/>
      </geometry>
    </visual>

    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>
  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0.2 0 0.07" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="hokuyo_link"/>
  </joint>


  <!-- ===============  Transmission =============== -->
  <transmission name="left_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_wheel_joint">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_wheel_motor">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>30</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_wheel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_wheel_joint">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_wheel_motor">
      <hardwareInterface>VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>30</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- =============== Gazebo =============== -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>my_robo</robotNamespace>
    </plugin>
  </gazebo>

  <gazebo reference="base_link">
    <selfCollide>true</selfCollide>
    <mu1 value="0.05" />
    <mu2 value="0.05" />
  </gazebo>

  <gazebo reference="left_wheel_link">
    <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

  <gazebo reference="right_wheel_link">
    <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

  <gazebo reference="caster_link">
    <selfCollide>true</selfCollide>
    <!-- This caster is no frictional resistance. -->
    <mu1 value="0.0" />
    <mu2 value="0.0" />
  </gazebo>

  <!-- hokuyo -->
  <gazebo reference="hokuyo_link">
    <sensor type="gpu_ray" name="head_hokuyo_sensor">
      <pose>0 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>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
        <topicName>/rrbot/laser/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>

これで、センサーをToFから平面レーザーへ載せ替え完了しました。前回作成したToFセンサーのコードを消し、新たに平面れレーザーの定義を追加しただけですね。

仮想3次元空間を用意する

次に、シミュレーションに使用する3次元空間を準備しましょう。今回は、gazeboに用意されている３Dデータを利用します。もちろん、３D CADが使えるようであれば自身で作成されてもよいでしょう。
それではGazeboを立ち上げてください。

$ sudo gazebo

sudoをつけて管理者で実行するのを忘れないようにしてください。sudoを付けないと、環境によっては保存時のウィンドウが正しく表示されません。
gazeboを立ち上げたら、左のペインのInsertタブでhttp://gazebosim.org/models/を展開します。ダウンロードされるので展開までちょっと時間がかかります。
展開されたら、そこからWillow Garegaを選び、右側のウィンドウでクリックして配置します。

これは、ROSを開発したWillow Garage社のオフィスです。ではこのまま保存しましょう。
メニューよりFile→Save World Asを選択します。
ファイル名をつけて保存します。今回はmy_robo_descriptionの中にworldフォルダを作り、willow_garage.worldという名前で保存しました。
gazeboを管理者で実行しているのでgazeboで作成したフォルダや保存したフィアルも管理者権限となります。あとで一般ユーザでもアクセスできるよう権限を変更しておいてください。

このように、好きな形状をgazebo上で作成して保存しておけば、シミュレーションに使うことができます。3DデータにはこのWillow Garage以外にもコンクリートブロックや、壁や、コーク缶などいろいろなものがありますので、シミュレータ上でテストしたい環境を速やかに構築することが可能です。

次は、作成したデータを読み込むためのlaunchファイルを作ります。
my_robo_descriptionパッケージのlaunchフォルダに移動して、willow.launchというファイルを作成してください。
roscdというコマンドを使うと、素早くパッケージのフォルダに移動することができます。

$ roscd my_robo_description
$ cd launch
$ gedit willow.launch

<launch>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find my_robo_description)/world/willow_garage.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>

</launch>

gazebo.launchファイルを次のように変更します。
変更前

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

変更後

  <include file="$(find my_robo_description)/launch/willow.launch">
  </include>

起動します。

$ roslaunch my_robo_description gazebo.launch

これでロボットがWillow Garageオフィス内を移動できる環境が整いました。

平面レーザーセンサーデータの確認

では、これから地図の作成を行います。実際のロボットでは、まず地図を知るために、対象となる場所を移動しながらセンサー情報を収集します。収集したセンサーの情報を用いて、地図を作成します。この地図を用いて、目的地へのナビケーションに使います。

まずセンサーの値が取れているか確認しておきましょう。

$ rosrun rviz rviz

rvizが起動したら、Fixed Frameをbase_footprintに設定し、AddボタンからBy topicタブの/rrbot/laser/scanのLaserScanを追加。
同様にBy display typeからTFを追加して表示します。

ロボットを動かしてみて、レーザーが検知した壁の様子が赤く表示されていればOKです。
ロボットを移動させてみましょう。次のように実行してください。

$ rosrun turtlesim turtle_teleop_key /turtle1/cmd_vel:=/my_robo/diff_drive_controller/cmd_vel

turtle_teleop_keyは、カーソルキーを押すことで、ロボットへの移動指示であるTwistメッセージを発行してくれるプログラムです。
通常は、/turtle1/cmd_velという名前でメッセージを発行しますが、作成したロボット用に/my_robo/diff_drive_controller/cmd_vel

という名前に変更してTwistメッセージを発行しています。

次に、センサーのデータを保存します。データの保存にはROSBAGというツールを使います。

ROSBAG

ROSBAGはROSメッセージの記録と再生を行います。記録されたデータは「バッグ」と呼ばれ、ロボットの位置、収集したデータ、カメラの画像などすべてROSメッセージとしてバブリッシュされていますが、それらをすべて（または指定したメッセージだけ）保存しておき、後で再現することができます。これによりデバッグなどの作業がはかどります。今回は、デプスカメラのデータをBagファイルに保存しておき、後で地図作成に使用します。

それではROSBAGで保存を開始しましょう。

$ mkdir ~/bagfiles
$ cd ~/bagfiles/
$ rosbag record -O my_robo_data /rrbot/laser/scan /tf

rosbagコマンドにrecordを指定すると記録を開始します。データはmy_robo_data.bagに保存されます。
トピックのうち/rrbot/laser/scan と /tf を記録するように指定しています。ちなみに、すべての記録を取る場合は「rosbag record --all」とします。

別ターミナルを開き、先ほどのturtle_teleop_keyでロボットを動き回らせて、センサーの値を収集しましょう。

オフィスは広いですが、Bagファイルは容量が大きくなるため、今回はほどほどにして終了しましょう。記録を終了するにはrosbagを実行しているターミナルで、Ctrl+Cを押します。


センサー情報からMapを構築する
マッピングに使うソフトウェアをインストールしていきます。

sudo apt-get install ros-indigo-gmapping
sudo apt-get install ros-indigo-openslam-gmapping
sudo apt-get install ros-indigo-slam-gmapping
sudo apt-get install ros-indigo-map-server

ターミナルを開き、

$ roscore

を実行します。また、別ターミナルを開き、

$ rosparam set use_sim_time true
$ rosrun gmapping slam_gmapping scan:=/rrbot/laser/scan

また別ターミナルを開き、

$ rosbag play --clock my_robo_data.bag

するとslam_gmappingを起動したターミナルに情報が表示され始めます。

このままrosbagが終了するまでしばらく待ちます。
rosbagの再生が終了したら、別ターミナルを開き、

$ rosrun map_server map_saver

すると、ファイルが生成されています。map.pgmとmap.yamlです。map.pgmを画像ビューワソフトなどで開くと、作成されたMapを確認することができます。

以上で、Mapの作成は完了です。次回は、このMapを使ってNavigationを行います。

ROSで始めるロボティクス（1） ー The Robot Operating System

ROSで始めるロボティクス（2） ー ROSのインストールとセットアップ

ROSで始めるロボティクス（3） ー 差動二輪ロボットを準備する

ROSで始めるロボティクス（4） ー シミュレータ上でロボットを動かしてみる

ROSで始めるロボティクス（5） ー GazeboでToFセンサーをシミュレーションする

ROSで始めるロボティクス（6） ー ROSでデプスカメラPMD CamBoard pico flexxを使う
ROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る
ROSで始めるロボティクス（8） ー ロボットのナビゲーションを行う

参考文献：

ROSではじめるロボットプログラミング ― 小倉 崇 (著) 

ROSプログラミング 単行本（ソフトカバー） ―  銭 飛 (著) 

詳説ROSロボットプログラミング ―  表 允晳, 倉爪 亮, 渡邊 裕太(著)　

http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData
http://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar
http://wiki.ros.org/navigation/Tutorials/RobotSetup

140 180 https://3.bp.blogspot.com/-SVQAb3FLhQk/V_7ZDLNYC_I/AAAAAAAABEg/CKvVPXERL8Q3l8xAFMAzkVRDq1AR9Hh3wCKgB/s1600/ros_start_title.png Gazebo , map_server , Navigation , ROS

記載されている会社名、および商品名等は、各社の商標または登録商標です。