#プログラミング ROS< 移動ロボットの作成（4） >

##はじめに
1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第31弾として，「移動ロボットの作成（4）」を扱う．

##環境
#####仮想環境


ソフト	VMware Workstation 15
実装RAM	2 GB
OS	Ubuntu 64 ビット
isoファイル	ubuntu-mate-20.04.1-desktop-amd64.iso

#####コンピュータ


デバイス	MSI
プロセッサ	Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz
実装RAM	8.00 GB (7.89 GB 使用可能)
OS	Windows (Windows 10 Home, バージョン：20H2)

#####ROS


Distribution	noetic
シミュレーション	gazebo

##移動ロボットの作成
ROSを使って，ほとんどの新しいロボットを制御する手順は次のようである．

1. ROSのメッセージインタフェースを決める．
2. ロボットのモータ用ドライバを書く．
3. ロボットの物理構造を書く．
4. Gazeboのシミュレーションで使用できるようにモデルに物理的特性を追加する．
5. tfを介して座標変換データを配信し，rvizでそれを可視化する．
6. センサを追加する．ドライバとシミュレーションのサポートも必要．
7. ナビゲーション等の標準的なアルゴリズムを適用する．

移動ロボットを例にその流れを確認していく．
今回は，5, 6についてまとめて移動ロボットの作成（4）として扱うこととする．

##自律移動
TortoiseBotを自律的に動かすには，幾つかの小さなステップを刻む必要がある．

可視化して座標変換を確認する．
レーザセンサを追加する．
ナビゲーションスタックを設定して組み込む．
ロボットの位置をrvizで特化してナビゲーションのゴールを設定する．

##手順5:座標変換を確認する
####rvizとGazebo
はじめに，Gazeboとrvizの役割について確認しておく．
Gazeboはロボットを模擬（シミュレート）する役割で，rvizはロボットを可視化する．rvizは，ロボットの思考に何が起きているのかを見せてくれる．一方でGazeboは何が本当に起きているのかを見せてくれる．つまり，rvizではセンサや経路などの中身を視覚的に確認することができ，Gazeboでは実際の挙動を見ることができるということである．

####座標変換
rvizも座標系間の関係を表す座標変換情報を必要とする．
この座標変換情報はtfトピックのtf2_msgs/TFMessageメッセージで提供される．このメッセージを提供するのは簡単で，次の2つのステップだけである．

ロボットすべての関節に関するsensor_msgs/JointStateメッセージを/joint_statesトピックに配信
robot_state_publisherで/joint_statesメッセージを対応する/tfメッセージに変換

関節状態を配信するには，Gazeboプラグインが使える．URDFファイルにそのプラグインを追加することで，関節状態を配信できるようになる．

###実装
まず，プラグインを追加する前の/joint_statesの一部を示す．

nameの部分が状態を配信している関節名になる．今の時点では右車輪と左車輪の関節の状態のみが配信されている．これは，前回差動駆動プラグインを追加したときにpublishWheelJointStateをtrueとしていたためである．このときrvizを起動させてロボットモデルを反映させようとすると...

よくわからない状態でロボットが組み立てられており，さらに左にあるパネルを見ると...

うまく座標変換ができていないというエラーが出ている．
次に関節状態配信プラグインを追加したURDFファイルのソースコードを示す．
####ソースコード

tortoisebot.urdf

<?xml version="1.0"?>
<robot name="tortoisebot">

	<!--台座のモデル-->
	<link name="base_link">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.6 0.3 0.3"/>
			</geometry>
			<material name="silver">
				<color rgba="0.75 0.75 0.75 1"/>
			</material>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.6 0.3 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="1.0"/>
          <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	
	<!--キャスターのモデル-->
	<link name="front_caster">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.1 0.1 0.3"/>
			</geometry>
			<material name="silver"/>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.1 0.1 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="0.00083" iyy="0.00083" izz="0.000167" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--台座とキャスターをつなぐ関節-->
	<joint name="front_caster_joint" type="continuous">
		<axis xyz="0 0 1"/>
		<parent link="base_link"/>
		<child link="front_caster"/>
		<origin rpy="0 0 0" xyz="0.3 0 0"/>
	</joint>

	<!--前輪のモデル-->
	<link name="front_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
      		</geometry>
      		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--前輪とキャスターをつなぐ関節-->
	<joint name="front_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="front_caster"/>
    	<child link="front_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="0.05 0 -.15"/>
    </joint>

    <!--右後方車輪のモデル-->
    <link name="right_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black">
    			<color rgba="0 0 0 1"/>
    		</material>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--右後方車輪と台車をつなぐ関節-->
    <joint name="right_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="right_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 -0.125 -.15"/>
    </joint>

    <!--左後方車輪のモデル-->
    <link name="left_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--左後方車輪と台車をつなぐ関節-->
    <joint name="left_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="left_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 0.125 -.15"/>
    </joint>

<!--機能的な部分をロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>left_wheel_joint</leftJoint>
      <rightJoint>right_wheel_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.25m，車輪直径0.07mとする-->
      <wheelSeparation>0.25</wheelSeparation>
      <wheelDiameter>0.07</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>

    <!--関節状態配信プラグインを指定-->
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <!--対象とする関節名を指定-->
      <jointName>front_caster_joint, front_wheel_joint</jointName>
    </plugin>
  </gazebo>
</robot>

####結果
rostopic echo /joint_statesにより，確認すると...

他の関節情報も配信されていることが分かる．
ここで．robot_state_publisherをlaunchファイルに追加する．

tortoisebot_rviz.launch

<launch>
	<!--TortoiseBotのURDFモデルをパラメータサーバにロードする-->
	<param name = "robot_description" textfile = "$(find make_robot1)/urdf/tortoisebot.urdf"/>

	<!--空のワールドでGazeboを開始する-->
	<include file = "$(find gazebo_ros)/launch/empty_world.launch"/>

	<!--GazeboでTortoiseBotを生成し，パラメータサーバからのその記述を受ける-->
	<node name = "spawn_urdf" pkg = "gazebo_ros" type = "spawn_model" args = "-param robot_description -urdf -model tortoisebot"/>
	
	<!--ロボットの関節状態を配信-->
	<node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "robot_state_publisher"/>
</launch>

さらにrvizでロボットモデルを反映させると...

うまく表示されており，エラーの部分は...

これもうまく解決されていることが確認できた．
なお，rostopic echo /tfでrobot_state_publisherに配信されているメッセージを調べることができるが，rviz上で行うと非常に簡単で視覚的に確認できる．

このとき，ロボットモデルのalpha値を0.5くらいにすると...

ロボットモデルが半透明になり，よりtf座標の表示が見やすくなる．

以上により，座標変換が正しく扱われていることを確認できた．

##手順6:レーザセンサを追加する
シミュレーションロボットにおいては，URDFに追加するだけで，センサを搭載できる．まず，センサを表現するリンクとそれをロボットに取り付けるための関節を追加．また，レーザにも適切な質量と慣性値を設定する必要がある．そうしないと，このレーザをGazeboのような物理ベースのシミュレーションに組み込むことができなくなる．

ここまででは，レーザの物理的な表現を追加しただけで，まだレーザとして振る舞うようにGazeboに指示していない．それには，＜sensor＞タグを使う必要がある．このタグは，センサを定義して設定することができる．

####ポイント

最初にgpu_rayタイプのセンサを作り，hokuyo_linkに取り付ける（gpu_rayはコンピュータのGPUを使ってセンサをシミュレーションすることを意味している．GPUはCPUよりかなり効率的）　　※ただし，GPUが使えない環境では，うまく作動しない．その場合，gpuタイプでないセンサを使う．なお，使用している仮想環境ではgpuのセンサはうまく作動しなかった．
次にこのセンサが北陽電機製のレーザと似た振る舞いをするように設定する．つまり，40Hzで新しいスキャンを配信し，180度の視野角に対して720点でサンプルし，最短0.1mから最大30mまでスキャンできるようにする．
最後にGPUレーザ用のGazeboプラグインを読み込んで，レーザからのデータをsensor_msgs/LaserScanメッセージとしてscanトピックに配信するように設定している．Gazeboプラグインに関する詳細な情報はgazebo_pluginsドキュメントを参照．（http://wiki.ros.org/gazebo_plugins）

###実装
先ほどのポイントをもとに変更を加えたURDFファイルのソースコードを示す．
#####ソースコード

tortoisebot.urdf

<?xml version="1.0"?>
<robot name="tortoisebot">

	<!--台座のモデル-->
	<link name="base_link">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.6 0.3 0.3"/>
			</geometry>
			<material name="silver">
				<color rgba="0.75 0.75 0.75 1"/>
			</material>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.6 0.3 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="1.0"/>
          <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	
	<!--キャスターのモデル-->
	<link name="front_caster">
        <!--外観-->
		<visual>
			<geometry>
				<box size="0.1 0.1 0.3"/>
			</geometry>
			<material name="silver"/>
		</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <box size="0.1 0.1 0.3"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="0.00083" iyy="0.00083" izz="0.000167" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--台座とキャスターをつなぐ関節-->
	<joint name="front_caster_joint" type="continuous">
		<axis xyz="0 0 1"/>
		<parent link="base_link"/>
		<child link="front_caster"/>
		<origin rpy="0 0 0" xyz="0.3 0 0"/>
	</joint>

	<!--前輪のモデル-->
	<link name="front_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
      		</geometry>
      		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
	</link>
	<!--前輪とキャスターをつなぐ関節-->
	<joint name="front_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="front_caster"/>
    	<child link="front_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="0.05 0 -.15"/>
    </joint>

    <!--右後方車輪のモデル-->
    <link name="right_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black">
    			<color rgba="0 0 0 1"/>
    		</material>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--右後方車輪と台車をつなぐ関節-->
    <joint name="right_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="right_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 -0.125 -.15"/>
    </joint>

    <!--左後方車輪のモデル-->
    <link name="left_wheel">
    	<visual>
    		<geometry>
    			<cylinder length="0.05" radius="0.035"/>
    		</geometry>
    		<material name="black"/>
    	</visual>
        <!--物理的特性-->
        <!--干渉-->
        <collision>
          <geometry>
            <cylinder length="0.05" radius="0.035"/>
          </geometry>
        </collision>
        <!--慣性-->
        <inertial>
          <mass value="0.1"/>
          <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
        </inertial>
    </link>
    <!--左後方車輪と台車をつなぐ関節-->
    <joint name="left_wheel_joint" type="continuous">
    	<axis xyz="0 0 1"/>
    	<parent link="base_link"/>
    	<child link="left_wheel"/>
    	<origin rpy="-1.5708 0 0" xyz="-0.2825 0.125 -.15"/>
    </joint>


    <!--センサ搭載の設定-->
    <link name="hokuyo_link">
      <!--可視化-->
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.1 0.1 0.1"/>
        </geometry>
      </visual>
      <!--物理的特性-->
      <!--干渉-->
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.1 0.1 0.1"/>
        </geometry>
      </collision>
      <!--慣性-->
      <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 0 0.2" rpy="0 0 0"/>
      <parent link="base_link"/>
      <child link="hokuyo_link"/>
    </joint>



<!--機能的な部分をロードするための追加要素-->
  <gazebo>
    <!--差動駆動プラグインを指定-->
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
      <!--left_wheel_jointとright_wheel_jointをそれぞれ左関節右関節に指定-->
      <leftJoint>left_wheel_joint</leftJoint>
      <rightJoint>right_wheel_joint</rightJoint>
      <!--台座であるbase_linkをrobotBaseFrameに指定-->
      <robotBaseFrame>base_link</robotBaseFrame>
      <!--トレッドを0.25m，車輪直径0.07mとする-->
      <wheelSeparation>0.25</wheelSeparation>
      <wheelDiameter>0.07</wheelDiameter>
      <!--車輪の状態を配信する設定にする-->
      <publishWheelJointState>true</publishWheelJointState>
    </plugin>

    <!--関節状態配信プラグインを指定-->
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <!--対象とする関節名を指定-->
      <jointName>front_caster_joint, front_wheel_joint</jointName>
    </plugin>
  </gazebo>

  <!--レーザセンサを定義する-->
  <gazebo reference="hokuyo_link">
    <sensor type="ray" name="hokuyo">
    <!--sensor type="gpu_ray" name="hokuyo"-->
      <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="laser" filename="libgazebo_ros_laser.so">
      <!--plugin name="gpu_laser" filename="libgazebo_ros_gpu_laser.so"-->
        <topicName>/scan</topicName>
        <frameName>hokuyo_link</frameName>
      </plugin>
    </sensor>
  </gazebo>

</robot>

#####シミュレーション
まず，rviz上でLaserScanを追加後，次に示すようにLaserScanのトピックを指定する．

空のワールド上に適当な障害物を置くと...

rvizの画面で障害物を検出できていることが確認できる．また先ほどのソースコードにおいて，センサのvisualizeタグをtrueにすると...

Gazebo上でもレーザの様子を確認することができる．

以上でセンサの追加及び，動作確認が終了した．

##感想
いよいよセンサを追加し，rvizにロボットモデルを反映することもできた．これにより，このロボットでもナビゲーションタスクを行う前準備ができた．また今回もURDFでの編集があり，そこそこ慣れてきて，ほかのセンサの追加もROSのチュートリアルを参考に実装できそうだと感じた．

##参考文献
プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行