Jetbotをgazeboで動かすまで その1

Jetbotという車輪型ロボットのRvizのみ対応のコードをgazeboに対応させて、シミュレーション上で動かせるようにした。

Rviz用のJetbotファイルをgazeboで動かすまでをざっとまとめた備忘録その1

目次
1. gazeboに対応させる手順
2. 今回の概要
3. gazeboプラグインについて
4. gazeboに対応させたJetbotのXacroファイル

1. gazeboに対応させる手順

gazeboでオリジナルロボットを動かすには次の手順が必要。

1. ロボットのSTL/COLLADAファイルの作成
ロボットの見た目を記述したファイル。
STLとかはCADで簡単に作れる。けどめんどいのでgithubのソースから拾ってくるのが入門としては一番。

2. ロボットのURDF（Xacro）ファイルの作成
ロボットをURDF（Xacro）で記述。

3. URDFにHardwareInterfaceを記述
とは動かすときに命令を与えるようのタグでJointタグの下に書く。
例

<transmission name="${prefix}_trans" type="SimpleTransmission">
     <type>transmission_interface/SimpleTransmission</type>
     <actuator name="${prefix}_motor">
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
     <joint name="${prefix}_joint">
       <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
     </joint>
</transmission>

4. URDFにgazeboプラグインの追加
これは次の記事でまとめて記述しますが、ROSとgazeboを連携したりするための記述。

5 その他
必要に応じてセンサープラグインや、ros_controllerのyamlファイルを記述。
これも詳細は次記事で。

6. launchファイルで起動
launchファイルは複数の処理を同時に実行できるので、gazeboは基本いろいろな機能が必要なのでlaunchファイルで起動する。

ROSとgazeboの連携の全体像

gazeboエレメントを追加することでrvizだけじゃなく、gazeboでもURDF(xacro)が読めるようになり、rvizとgazeboの両方を使えるようになり多彩なシミュレーションが可能に

2. 今回の概要

今回はRvizでは表示できるけど、gazeboでは動かせないJetbotを動かせるようにします。
ちなみにjetbotは車輪型ロボット。

・まずURDFにロボットの各軸の動軸、自由軸の設定を記述。

・そこにros_controlというgazeboプラグインを使ってROSからの指令でシミュレーター上で動くようにする。

・2つの動輪がROSからTwistを受け取って動くようにする。

そのためには以下の図のように.urdfに記述を加えてHardwareInterfaceを記述することとLaunchでControllerManagerの設定をすることが必要。

3. gazeboプラグイン

gazenoはもともとはROSとは別のソフト。

ROSからgazeboを動かそうとしたらROSとgazeboを接続する橋渡し的なプラグインの記述が必要でこれが「gazeboプラグイン」。

今回使うROSコントロールもgazeboプラグインの1つで、ROSトピックをsubscribeしてgazeboシミュレーター中の関節を動かします。またhokuyoセンサー(Lidar)やcameraなどのプラグインも接続させてシュミレーションを実行します。

逆にgazenoシミュレーター中の関節の角度をROSトピックにpublishするプラグインもあります。これ以外にも多くのgazeboプラグインが多く公開されてます。

hokuyoセンサーのプラグインの例(hokuyo.urdf.xacro)

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hokuyo">
<material name="yellow" >
    <color rgba="1.0 1.0 0.0784313725490196 1.0" />
</material>	
<xacro:macro name="hokuyo_laser" params="xyz rpy parent length radius">
    
    <joint name="hokuyo_laser_joint" type="fixed">
        <axis xyz="0 0 1" />
        <origin xyz="${xyz}" rpy="${rpy}"/>
        <parent link="${parent}"/>
        <child link="hokuyo_laser_link"/>
    </joint>
     
    <link name="hokuyo_laser_link">
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
		    <cylinder length="${length}" radius="${radius}"/>
	    </geometry>
        </collision>
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <cylinder length="${length}" radius="${radius}"/>
	    </geometry>
	    <material name="yellow"/>
        </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>
    <gazebo reference="hokuyo_laser_link">
        <material>Gazebo/Yellow</material>
    </gazebo>
    <gazebo reference="hokuyo_laser_link">
        <gravity>true</gravity>
        〜略〜
                <frameName>hokuyo_laser_link</frameName>
            </plugin>      
        </sensor>
    </gazebo>
 
</xacro:macro>
</robot>

Jetbotの取り付けるURDFファイルの記述

<xacro:include filename="$(find jetson_description)/meshes/sensor/hokuyo.urdf.xacro"/>

<!-- Laser link and plugin -->
<xacro:hokuyo_laser xyz="0 0 ${0.04 + 0.02}" 
	  rpy="0 0 0"
	  parent="base_link" length="0.01" radius="0.015">
</xacro:hokuyo_laser>

4. gazeboに対応させたJetbotのXacroファイル

Rvizで表示させるとこんな感じ。

これをgazeboで動かせるようにしたXacroファイル

<?xml version="1.0"?>
<robot name="jetbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Included URDF/XACRO Files -->
  <xacro:include filename="$(find jetson_description)/urdf/common.xacro" />
  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
  <xacro:include filename="$(find jetson_description)/urdf/jetbot.gazebo" />
  <xacro:include filename="$(find jetson_description)/meshes/sensor/hokuyo.urdf.xacro"/>

  <!-- chassis dimensions -->
  <xacro:property name="PI" value="3.1415926535897931"/>
  <xacro:property name="mass" value="1.0" /> <!-- mass body-->
  <xacro:property name="width" value="0.05" /> <!-- Link 1 -->
  <xacro:property name="height" value="0.03" /> <!-- Link 1 -->
  <xacro:property name="length" value="0.10" /> <!-- Link 1 -->

  <!-- wheel dimensions -->
  <xacro:property name="wheel_mass" value="0.1" /> <!-- mass wheel -->
  <xacro:property name="wheel_radius" value="0.03" /> <!-- radius -->
  <xacro:property name="wheel_width" value="0.01" /> <!-- wheel --> 

  <xacro:property name="camera_link" value="0.01" /> <!-- Size of square 'camera' box -->
  <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->

  <!-- Default wheel joint-->>
  <xacro:macro name="wheel_macro" params="parent prefix xyz color">
    <joint name="${prefix}_joint" type="continuous">
      <parent link="${parent}"/>
      <child link="${prefix}_link"/>
      <origin xyz="${xyz}" rpy="0 0 0"/>
      <axis xyz="0 -1 0"/>
    </joint>
    <transmission name="${prefix}_trans" type="SimpleTransmission">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${prefix}_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${prefix}_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>
    <link name="${prefix}_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 1.57079632679"/>
      <geometry>
	      <mesh filename="package://jetson_description/meshes/DAE/JetBot-v3-Wheel222.dae"/>
      </geometry>
      <material name="${color}"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 1.57079632679"/>
      <geometry>
	      <mesh filename="package://jetson_description/meshes/DAE/JetBot-v3-Wheel222.dae"/>
      </geometry>
    </collision>

      <inertial>
        <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
        <mass value="${wheel_mass}" />
        <xacro:cylinder_inertia  m="${wheel_mass}"
          r="${wheel_radius}" h="${wheel_width}" />
      </inertial>
    </link>
    <gazebo reference="${prefix}_link">
      <mu1>0.2</mu1>
      <mu2>0.2</mu2>
      <kp value="50000" />
      <kd value="10" />
      <material>Gazebo/white</material>
    </gazebo>
  </xacro:macro>

  <!-- Used for fixing robot to Gazebo 'base_link' -->
  <!-- base_footprint Definition -->
  <link name="odom"/>

  <joint name="odom_footprint" type="fixed">
    <parent link="odom" />
    <child  link="base_footprint" />
  </joint>

  <link name="base_footprint" />

  <joint name="footprint_base" type="fixed">
    <parent link="base_footprint" />
    <child  link="base_link" />
    <origin xyz="0.0 0.0 0.030"/>
  </joint>

  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	      <mesh filename="package://jetson_description/meshes/DAE/JetBot-v3-Chassis444.dae"/>
      </geometry>
      <material name="white"/>
    </visual>

    <collision>
      <origin xyz="-0.03 0 0.03" rpy="0 0 0"/>
      <geometry>
	<box size="${length} ${width} ${height}"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${mass}" />
      <origin xyz="-0.06 0 0.04" rpy="0 0 0" />
      <xacro:box_inertia m="${mass}" x="${length}" y="${width}" z="${height}" />
    </inertial>

  </link>

  <!-- right_wheel and joint-->
  <xacro:wheel_macro parent="base_link" prefix="right_wheel" xyz="0 -0.053 0" color="black"/>

  <!-- left_wheel and joint-->
  <xacro:wheel_macro parent="base_link" prefix="left_wheel" xyz="0 0.047 0" color="orange"/>

  <!-- caster joint-->
  <joint name="caster_0_joint" type="fixed">
    <parent link="base_link"/>
    <child link="caster_0"/>
    <origin rpy="0 0 0" xyz="-0.08 0 -0.018"/>
  </joint>

  <!-- caster -->
  <link name="caster_0">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.012"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.012"/>
      </geometry>
    </collision>

      <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value="0.1" />
        <xacro:sphere_inertia  m="0.1" r="0.012" />
      </inertial>

  </link>

  <gazebo reference="caster_0">
    <turnGravityOff>false</turnGravityOff>
    <material>Gazebo/White</material>
    <mu1 value="0.0"/>
    <mu2 value="0.0"/>
  </gazebo>

  <!-- Camera joint-->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 0 1" />
    <origin xyz="0.025 0 0.06" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>

  <!-- Camera -->
  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
      <material name="red"/>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
	<box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="0.1" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <xacro:box_inertia m="0.1" x="${camera_link}" y="${camera_link}" z="${camera_link}" />
    </inertial>

  </link>

  <!-- Laser link and plugin -->
  <xacro:hokuyo_laser xyz="0 0 ${0.04 + 0.02}" 
	  rpy="0 0 0"
	  parent="base_link" length="0.01" radius="0.015">
  </xacro:hokuyo_laser>

  <!-- gazebo plugins  -->
  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/jetbot</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>

  <!-- Odometry plugin-->
  <gazebo>
    <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
      <frameName>world</frameName>
      <bodyName>base_link</bodyName>
      <topicName>/tracker</topicName>
      <updateRate>10.0</updateRate>
    </plugin>
  </gazebo>

</robot>

参考

・ROS講座39 車輪ロボットを作る3(diff_drive_controllerで動かす)