gazeboを使いこなす！

gazebo上で自作のロボットモデルを走行させる．

• gazeboとは
• gazeboに表示するモデルの作成
• gazebo上で駆動制御を行う
• 状態をrvizで表示する

gazeboとは

Gazeboでは，主にSDF（Simulation Description Format）というフォーマットを採用しており，それを用いてでモデルやシミュレーション環境を定義している．これは，URDFの「閉リンク機構に対応していない」という問題を解決するためだが，SDFもSDFで課題がある．

特に，
• URDFでも，タグを使えばURDFと同じ内容（必要な内容）を記述できること
• URDFのほうが，SDFより書きやすく，xacroという使いやすいツールもあること
という理由から，URDFで記述する人が多いらしい．

そこで，今回はxacroファイルを用いたURDFによって記述していく．（複雑なURDFを書くときにおすすめツール：VScode拡張機能）

gazeboに表示するモデルの作成

gazeboモデルとrvizモデルの最も大きな違いは，物理情報の有無である．
rvizでは見た目の形と色のみ設定していたが，物理シミュレーションを行うにあたって以下の3つの情報が追加で必要になる．

• collision：衝突判定に用いる物体の形状を設定
• inertial：衝突時の状態計算に用いる慣性モーメントを設定
• gazebo reference：（☝２つとは異なり）gazebo内で表示するモデルの色を設定．シミュレーション環境の重力設定も行える．

	rviz	gazebo
joint	origin，parent_link，child_link	origin，parent_link，child_link
link	visual	visual + collision，inertial
その他	-	gazebo reference

以下，手順
1-1. パッケージの新規作成
1-2. 各部品のxacroファイルを入れるフォルダを作成
1-3. 作成したurdfをrvizで確認
1-4. gazeboで表示する

1. パッケージの新規作成

xacroでロボットを定義するパッケージを作る．慣習的に「*_description」という名前にする．

$ cd ~/practice_ws/src/
$ mkdir robot_lecture
$ catkin_create_pkg robot_description std_msgs rospy roscpp tf sensor_msgs urdf xacro joint_state_publisher robot_state_publisher

2. 各部品のxacroファイルを入れるフォルダを作成

今回作るロボットは，以下3部品+α で構成される対向2輪ロボットとする．

• chassis：ロボットの車体．直方体の部品からなる．
• wheel：車輪．薄い円柱部品からなる．これを駆動させることで移動できる．
• caster：非駆動輪．小さな球体部品からなり，駆動輪の動きに応じて自然に動く．
• common：色情報や形に応じた慣性モーメントの計算式などのすべての部品で使う共通コードを記載．
  

一つのurdfによって長々と書くより，xacroファイルを用いて，ロボットの部品ごとに分割して記述すると分かりやすい．（xacroについてはこちら）

$ cd robot_description
$ mkdir -p urdf/chassis
$ cd urdf
$ mkdir wheel caster robot common

common

形状ごとの慣性モーメントの計算式や色の定義など，すべての部品で共通の情報についてはここにまとめてある．

```xml:common/common.urdf.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://<ros.org/wiki/xacro">
	<!-- PROPERTY LIST -->
	  <!-- Inertial. Units are kg*m^2 -->
	  <xacro:macro name="box_inertia" params="m x y z">
	    <inertia ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
	             iyy="${m*(z*z+x*x)/12}" iyz = "0"
	             izz="${m*(x*x+y*y)/12}" /> 
	  </xacro:macro>
	  <xacro:macro name="cylinder_inertia" params="m r h">
	    <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
	             iyy="${m*(3*r*r+h*h)/12}" iyz="0"
	             izz="${m*r*r/2}" />
	  </xacro:macro>
	  <xacro:macro name="sphere_inertia" params="m r">
	    <inertia ixx="${m*r*r/5}" ixy="0" ixz="0"
	             iyy="${m*r*r/5}" iyz="0"
	             izz="${m*r*r/5}" /> 
	  </xacro:macro>
	
	  <!-- Color -->
	  <material name="blue">
	    <color rgba="0.0 0.0 0.8 1.0"/>
	  </material>
	  <material name="green">
	    <color rgba="0.0 0.8 0.0 1.0"/>
	  </material>
	  <material name="red">
	    <color rgba="0.8 0.0 0.0 1.0"/>
	  </material>
	  <material name="white">
	    <color rgba="1.0 1.0 1.0 1.0"/>
	  </material>
	  <material name="yellow">
	    <color rgba="0.8 0.8 0.0 1.0"/>
	  </material>
	
</robot>
```

chassis

1.0x0.5x0.2 mの黄色い直方体を表示する．

chassis/chassis.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
//直方体の大きさを事前設定
   <xacro:property name="size" value="1.0 0.5 0.2" /> 
  	
//"chassis"という名前のマクロを作成する．
//ただし，parent linkとjoint位置はマクロ使用時に代入できるようにする
   <xacro:macro name="chassis" params="parent *joint_origin">
	<joint name="chassis_joint" type="fixed">
		<xacro:insert_block name="joint_origin"/>
		<parent link="${parent}"/>
		<child link="chassis_link"/>
	</joint>

	<link name="chassis_link">
		<visual>
			<geometry>
				<box size="${size}"/>
			</geometry>
			<material name="yellow"/>
		</visual>

        //以下，gazeboで使うために追加で必要な情報（collision，inertial，gazebo reference）
		<collision>
			<geometry>
				<box size="${size}"/>
			</geometry>
		</collision>

		<inertial>
			<origin xyz="0 0 0"/>
			<mass value="0.6"/>
			<xacro:box_inertia m="0.6" x="1.0" y="0.5" z="0.2"/>
		</inertial>
	</link>
    //以下のコードがないと，gazeboで色を表示できない
	<gazebo reference="chassis_link">
			<material>Gazebo/Yellow</material>
			<turnGravityOff>false</turnGravityOff>
	</gazebo>
  </xacro:macro>
</robot>

wheel

直径0.1m，長さ0.05mの赤い円柱を表示する．

wheel/wheel.urdf.xacro

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

	  <xacro:macro name="wheel" params="prefix parent *joint_origin *joint_axis">
	    <joint name="${prefix}_wheel_joint" type="continuous">
	      <xacro:insert_block name="joint_origin" />
	      <xacro:insert_block name="joint_axis" />
	      <parent link="${parent}" />
	      <child link="${prefix}_wheel_link" />
	    </joint>
	    
	    <link name="${prefix}_wheel_link">
	      <visual>
	        <geometry>
	          <cylinder radius="0.1" length="0.05"/>
	        </geometry>
	      </visual>
	      
	      <collision>
	        <geometry>
	          <cylinder radius="0.1" length="0.05"/>
	        </geometry>
	      </collision>
	      
	      <inertial>
	        <origin xyz="0.0 0.0 0.0" />
	        <mass value="0.500"/>
	        <xacro:cylinder_inertia m="0.5" r="0.1" h="0.05"/>
	      </inertial>
	    </link>
		<gazebo reference="${prefix}_wheel_link">
			<material>Gazebo/Red</material>
			<turnGravityOff>false</turnGravityOff>
		</gazebo>
	  </xacro:macro>
</robot>

caster

直径0.05mの赤い球体を表示する．

caster/caster.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
	  <xacro:macro name="caster" params="parent *joint_origin">
	    <joint name="$caster_joint" type="fixed">
	      <xacro:insert_block name="joint_origin" />
	      <parent link="${parent}" />
	      <child link="caster_link" />
	    </joint>
	
	    <link name="caster_link">
	      <visual>
	        <geometry>
	          <sphere radius="0.05"/>
	        </geometry>
	      </visual>
	
	      <collision>
	        <geometry>
	          <sphere radius="0.05"/>
	        </geometry>
	      </collision>
	
	      <inertial>
	        <origin xyz="0.0 0 0.0" />
	        <mass value="0.50" />
	        <xacro:sphere_inertia m="0.5" r="0.05"/>
	      </inertial>
	    </link>
		<gazebo reference="caster_link">
			<material>Gazebo/Red</material>
			<turnGravityOff>false</turnGravityOff>
		</gazebo>
	  </xacro:macro>
</robot>

robot

各部品についてのxacroファイルを読み込み，それらを組み立てる．

robot/robot.urdf.xacro

<?xml version="1.0"?>
	<robot name="myrobot" xmlns:xacro="http://ros.org/wiki/xacro">
	  <xacro:include filename="$(find robot_description)/urdf/robot/common.urdf.xacro"/>
	   <xacro:include filename="$(find robot_description)/urdf/chassis/chassis.urdf.xacro"/>
	  <xacro:include filename="$(find robot_description)/urdf/wheel/wheel.urdf.xacro"/>
	  <xacro:include filename="$(find robot_description)/urdf/caster/caster.urdf.xacro"/>

	  <link name="base_footprint"/>
	
	  <xacro:chassis parent="base_footprint">
	    <origin xyz="0 0 0"/>
	  </xacro:chassis>
	
	  <xacro:wheel prefix="right" parent="chassis_link">
	    <origin xyz="0.25 -0.3 0.0" rpy="1.57 0 0" />
	    <axis xyz="0 0 -1" />
	  </xacro:wheel>
	  
	  <xacro:wheel prefix="left" parent="chassis_link">
	    <origin xyz="0.25 0.3 0.0" rpy="-1.57 0 0" />
	    <axis xyz="0 0 1" />
	  </xacro:wheel>
	
	  <xacro:caster parent="base_link">
	    <origin xyz="-0.15 0 -0.1"/>
	  </xacro:caster>
</robot>

3. 作成したurdfをrvizで確認

作成したurdfのあるディレクトリに移動してから，以下のコマンドを入力．

$ roslaunch urdf_tutorial display.launch model:=robot.urdf.xacro

エラー表示が出ても，
Global Options>「Fixed frame」をbase_linkからbase_footprintに変更すればOK（下矢印を開くと，選べるはず）

☟のように表示されればOK．

※launchファイルからちゃんと書きたい場合は，rvizを使いこなす！を参照．

4. gazeboで表示する

gazeboを作成したモデルで起動するlaunchファイルを記述する．

$ mkdir launch
$ cd launch
$ vim gazebo_display.launch

gazebo_display

<launch>
	<!-- Gazeboを起動する -->
	  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

	<!-- xacroファイルをurdfに展開して，パラメータに格納 -->
	<param name="robot_description" command=$"(find xacro)/xacro $(find robot_description)/urdf/robot/robot.urdf.xacro"/>

	<!-- "robot_description"パラメータに格納されたurdfをもとに，gazebo上にモデルを作成するプログラムを起動 -->
	<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
	    args="-param robot_description -urdf -model myrobot"/>
</launch>

$ roslaunch robot_description rviz_display.launch

☝このように表示されればOK！

gazebo上で駆動制御を行う

状態をrvizで表示する

1. rviz上の車輪を回転させる
2. rviz上の車両を移動させる

rviz上の車輪を回転させる

joint位置をgazeboから出力させるために，以下のコードをlaunchファイルに追記する．

launch/gazebo_display.launch

	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

gazeboを起動したうえで，もう一度rvizを表示してみる

$ roslaunch robot_description gazebo_display.launch
$ rviz

さらに移動させてみる

$ roslaunch robot_description gazebo_display.launch
$ rviz
$ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'

☝rviz側の車輪が動けばOK！

rviz上で車輪の動きを確認するとき，
"Axes"や"TF"を追加して車輪の軸を表示させると分かりやすい．

rviz上の車両を移動させる

車輪の回転に用いているdifferential_drive_controllerに車両の位置情報をpublishさせればよい．

robot.urdf.xacro

<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
        <!-- for running robot in gazebo -->
        <leftJoint>left_wheel_joint</leftJoint>
        <rightJoint>right_wheel_joint</rightJoint>
        <robotBaseFrame>base_footprint</robotBaseFrame>
        <wheelSeparation>0.5</wheelSeparation>
        <wheelDiameter>0.15</wheelDiameter>
        <publishWheelJointState>true</publishWheelJointState>
    
        <!-- for running robot in rviz -->
        <odometryFrame>world</odometryFrame>
        <publishWheelTF>true</publishWheelTF>
        <publishOdomTF>true</publishOdomTF>
        <odometrySource>world</odometrySource>
        <publishTf>true</publishTf>
    </plugin>
</gazebo>

gazeboを起動したうえで，もう一度rvizを表示してみる

$ roslaunch robot_description gazebo_display.launch
$ rviz

さらに移動させてみる

$ roslaunch robot_description gazebo_display.launch
$ rviz
$ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'

☝のように，rvizの原点を表すworld frameから車両が離れていけばOK!

トラブルシューティング

Error [Err] [REST.cc:205] Error in REST request

[gazebo-2] process has died [pid 8983, exit code 255

結構，何度も直面するエラー

$ killall gzserver
$ killall gzclient