環境

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

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

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

概要

Gazeboでシミュレーションしている位置を/tfに取り出しそれをRvizで表示します。

ホイールオドメトリーの位置を表示

タイヤについているエンコーダーの回転数を計算していればその時々の速度が取得でき、それを積算すれば起動時点からの相対の位置を取得できます。これをホイールオドメトリーといいます。移動ロボットならセンサーを付ければ取れる値なのでよく使われます。これに相当する値をGazeboから取得します。

ソースコード

以下のlaunchでGazeboとRvizを起動します。
Gazeboで読み込むModelでは、既にWheelOdometryの値を出力する設定がされています。追加でRvizの追加の設定を書いています。

gazebo1_lecture/launch/wheel_robot_simple.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find gazebo1_lecture)/urdf/wheel_robot_simple.urdf"/>   
  <arg name="rvizconfig" default="$(find gazebo1_lecture)/rviz/wheel_robot_simple.rviz" />
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="wheel_robot_simple.world"/>
    <arg name="verbose" value="true"/>
  </include>

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

実行

以下を実行します。

roslaunch gazebo1_lecture wheel_robot_simple.launch

Gazebo上で移動した状況がRviz上に表示されます。ただしRvizではWheelOdometryでタイヤの回転数から位置を計算しているにすぎません。例えば機体自体を手で持って移動するなど、タイヤの回転が伴わない移動では位置が反映されません。

真位置の取得

WheelOdometryではなく、真の位置を取るためにはGazeboのモデルプラグインであるlibgazebo_ros_p3dを使う必要があります。

ソースコード

libgazebo_ros_p3dを使っています。

gazebo1_lecture/models/tracked_box/model.sdf

<?xml version='1.0'?>
<sdf version="1.4">
  <model name="tracked_box">
    <pose>0 0 0.5 0 0 0</pose>
      <link name="box_link">
      <inertial>
        <mass>1.0</mass>
        <ixx>0.083</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.083</iyy>
        <iyz>0.0</iyz>
        <izz>0.083</izz>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Red</name>
          </script>
        </material>
      </visual>
    </link>
    <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
      <frameName>world</frameName>
      <bodyName>box_link</bodyName>
      <topicName>/tracker</topicName>
      <updateRate>10.0</updateRate>
    </plugin>
  </model>
</sdf>

最後の方の<plugin>タグでp3dプラグインを有効にしています。

これはあるリンクからあるリンクまでの相対位置をnav_msgs/Odometryのrostopicで出力するプラグインです。
<frameName>-><bodyName>までの相対位置を出力します。両方とのSDF中のlink名で指定します。ただしframeNameがworldかmapの時はGazeboワールドの原点からの位置を出力します。またこれらの名前はodometryのmsg中に入ります。
<topicName>は出力するOdometryのtopic名を指定します。
<updateRate>出力レートを指定します。

gazebo1_lecture/worlds/tracked_box.world

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://tracked_box</uri>
    </include>
  </world>
</sdf>

gazebo1_lecture/launch/tracked_box.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="rvizconfig" default="$(find gazebo1_lecture)/rviz/tracked_box.rviz" />

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="tracked_box.world"/>
    <arg name="verbose" value="true"/>
  </include>

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

実行

実行

roslaunch gazebo1_lecture tracked_box.launch

gazebo上で箱を動かすと、それに応じた位置がRvizに表示されます。

コメント

出来ることなら真位置を使った方がロボットの制御がしやすいです。しかし現実のロボットで真位置を使うためにはモーショントラッカーなどのリッチな環境が必要で、実験室でない限り実環境では望めません。
実環境で使うことを考えるとwheel odometryを使うほうが現実的で、徐々にずれてしまうなどの問題にはLIDARを使ったSLAMと組み合わせる等して対処します。これらの技法は「自己位置推定(Localization)」というロボットの一大分野となります。

目次ページへのリンク

ROS講座の目次へのリンク