ROS勉強メモ Gazeboシミュレーション

概要
=
GazeboはROSと連携できるシミュレーションツール。GUI表示用のURDFモデルに今度シミュレーション用の記述（重さ、慣性モーメントなど）を追加しておけば、Gazeboで物理的なシミュレーションできるようになる。

絵の右にはRobotの制御本体、左の部分はシミュレーション。Robot本体の動きをシミュレーション上で実現できるように制御のIFは共通するイメージ。カメラセンサでGazebo上の疑似環境を入力されread()→ROSのRobot　APPで判断し、障害物を避ける計算した上、操舵目標値を算出→ROS controlで操舵目標値を実際にハードが認識できる制御値に変換→write()操舵して、障害物を回避するまでの流れがこれでシミュレーションできる、見える。
![キャプチャ.PNG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/1fe6283c-4774-280c-b003-e64652f3638c.png)

準備（インストール/アンインストール
=
ROSをFullでインストールしている場合はインストール不要だが、そうじゃなければ、下記参照してインストールする必要がある。

https://classic.gazebosim.org/tutorials?cat=guided_b&tut=guided_b1

わかりにくい場合はこれでもインストールできる
Gazeboのインストール
~~~~~~~~~~~~
wget -O gazebo.sh http://get.gazebosim.org/
sh gazebo.sh
~~~~~~~~~~~~
GazeboとROSの連携ツールのインストール
~~~~~~~~~~~~
sudo apt install ros-noetic-gazebo-ros-pkgs  ros-noetic-gazebo-msgs  ros-noetic-gazebo-plugins  ros-noetic-gazebo-ros-control
~~~~~~~~~~~~

インストール成功したか、ROSとの連携ができたかを確認しておく。
ターミナル1
~~~~~~~~~~~~
roscore
~~~~~~~~~~~~
ターミナル2
~~~~~~~~~~~~
rosrun gazebo_ros gazebo
~~~~~~~~~~~~
ターミナル3
~~~~~~~~~~~~
rostopic list
~~~~~~~~~~~~
Gazeboが起動できていること、rostopicにgazeboも入っていればインストールが完了。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/184ed42a-0e2e-e422-91f2-43ce1848790b.png)

※バージョン間違った場合はアンインストールコマンド
~~~~~~~~~~~~
sudo apt-get remove gazebo-*
~~~~~~~~~~~~

Gazeboをインストールするだけで環境モデルを入れないとうまく表示しないことがあるから、事前にモデルをダウンロードする。格納先は~/.gazebo/models。※homeから通常見当たらないが、Ctrl+hすれば隠しフォルダが表示される。
ダウンロードしない場合は通常onlineのモデルを読みに行ってくれるが、遅かったりするかもしれない。
https://github.com/osrf/gazebo_models.git

若しくは下記コマンドを使いダウンロードする
~~~~~~~~~~~~~~~
cd ~/.gazebo/
mkdir -p models
cd ~/.gazebo/models/
wget http://file.ncnynl.com/ros/gazebo_models.txt
wget -i gazebo_models.txt
ls model.tar.g* | xargs -n1 tar xzvf
~~~~~~~~~~~~~~~


準備（画面を確認しておく
=
ターミナルにコマンド[gazebo]を入れておけば、画面が起動する
![Screenshot from 2023-06-24 20-20-31.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/9942306c-8c50-9c1c-a5a7-9225132a13e5.png)

![7f67618ac4b449c8afc20aac492d87f7.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/528115a4-e59e-2b1c-8ce4-81b243e22c2c.png)

![ebd8fad7614545319c208ced95eaddd2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/13f2a6b4-6936-d377-0814-2e9399ab869b.png)

注意：
VisualBoxを使う場合はディスプレイ設定の3Dアクセラレーションを有効化にしていると、Gazeboが異常になる場合がある。

ステップ１(既存URDFモデルにGazebo対応する)
=
![68747470733a2f2f71696974612d696d6167652d73746f72652e73332e61702d6e6f727468656173742d312e616d617a6f6e6177732e636f6d2f302f333138313230382f36616136373833332d316437642d303637362d643937632d3330363765333432646565312e706e67.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/4f134ef3-380e-fb34-d33b-4c6485458aef.png)
Visualは表示用ラベルだが、物理的なシミュレーションするためInertialとCollisionラベルが必要になる。

1.Linkにシミュレーションラベル追加
◇慣性モーメンxmlラベル：
~~~~~~~~~~~~~~~~~~~
<inertial>
    <mass value="2" /> 
    <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5"> 
    <origin xyz="..." rpy="..." />
</inertial>
~~~~~~~~~~~~~~~~~~~
◆mass：重さ(kg),重さを入れないと物体の質量が0、動力に対して物体の移動が無駄に伸びるから、できれば近い値を入れる
◆inertia：慣性モーメント3x3の配列だが、対称しているから半分だけ記述すればよい。値はどうやって決めているの？これはかなり難しい話、剛体力学物理を勉強しましょう。
◆origin：座標位置、記述しなければ、default値になる。xyz(m),位置。rpy(°),xyz軸に対する回転角度

◇衝突係数xmlラベル：
（１つのLinkは複数のCollisionを設定できる）
~~~~~~~~~~~~~~~~~~~
<collision name="..."> 
    <geometry>
        <box>
    </geometry>
    <origin xyz="..." rpy="..." />
</collision>
~~~~~~~~~~~~~~~~~~~
◆geometry：Visualの中でもあるのに、何で必要？まずこの疑問があったが、Visualの中のは表示用の形、例えば、不規則なロボット頭部を表したとしてもシミュレーションするためにはそこまで複雑な形状でやる必要がなくて、真ん丸、若しくはキューブ形状にした方が設定すれば大体のシチュエーションでは満足できるので。
　・box：キューブ形状.
　・cylinder：円柱.
　・sphere：球体.
　・mesh：filename="package://****.dae" /> meshファイルを導入する
◆origin：位置情報

◇Gazeboラベル：
~~~~~~~~~~~~~~~~~~~
<gazebo reference="base_link">
    <material>Gazebo/Black</material>
</gazebo>
~~~~~~~~~~~~~~~~~~~
Gazeboの中に定義してくれた色があるので、reference部位”Link名”に”Gazebo/Black”色を指定する感じ。Visualの中色はRVitしか認識しないので、Gazeboが認識する色と兼用できなく、使う色の定義も異なる。

2.Jointに動力転導ラベル(transmission)追加
RobotモデルをGazeboの中で動かすのにControllerを追加し、Controllerを使いTransmissionを介してJointを動かすことになるイメージなので、URDFファイルに動かす部位にTransmissionラベルがMust。例えば、タイヤ回転させたい、アームを動かしたい場合。

~~~~~~~~~~~~~~~~~~~
<transmission name="simple_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="foo_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="foo_motor">
    <mechanicalReduction>50</mechanicalReduction>
    <hardwareInterface>VelocityJointInterface</hardwareInterface>
  </actuator>
</transmission>
~~~~~~~~~~~~~~~~~~~
◆transmission（must）joint nameと同じ名前を設定すればよい
◆type（1つだけ）転導タイプ、例としてtransmission_interface/SimpleTransmission
◆joint（複数設定OK）これはJointを指定し、ControllerのhardwareInterfaceを設定するものここは例としてhardwareInterfaceのEffortJointInterfaceを設定している。
◆hardwareInterface（jointに対して設定するが、複数設定OK）IFを設定している
　・position (関節に位置指令IF)
　・velocity (関節に速度指令IF)
　・effort (関節に力指令IF)
◆actuator（複数設定OK）駆動源
◆mechanicalReduction（オプション）減速比

3.Gazebo Pluginラベル追加
Pluginは何？ROSはロボットのアクチュエーター・センサーのシミュレーション用に色んなPluginを提供されている。下記フォルダ内に.soファイルが格納されている。APIのイメージ。pluginを入れて、必要なパラメータを入れておけば、使いたい機能がGazebo上でシミュレーションできる。

![Screenshot from 2023-06-25 21-09-42.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/c9f2e0b0-38ff-d3a2-6a2c-4e1787b8855f.png)

これで差動ドライブのプラグイン。libgazebo_ros_diff_drive.soを使うことを最初includeしているイメージで、下はこの.soが使うパラメータ。
~~~~~~~~~~~~~~~~~~~~~~~~
        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
                <rosDebugLevel>Debug</rosDebugLevel>
                <publishWheelTF>true</publishWheelTF>
                <robotNamespace>/</robotNamespace>
                <publishTf>1</publishTf>
                <publishWheelJointState>true</publishWheelJointState>
                <alwaysOn>true</alwaysOn>
                <updateRate>100.0</updateRate>
                <legacyMode>true</legacyMode>
                <leftJoint>base_to_wheel_left_joint</leftJoint>
                <rightJoint>base_to_wheel_right_joint</rightJoint>
                <wheelSeparation>${base_link_radius*2}</wheelSeparation>
                <wheelDiameter>${2*wheel_radius}</wheelDiameter>
                <broadcastTF>1</broadcastTF>
                <wheelTorque>30</wheelTorque>
                <wheelAcceleration>1.8</wheelAcceleration>
                <commandTopic>cmd_vel</commandTopic>
                <odometryFrame>odom</odometryFrame> 
                <odometryTopic>odom</odometryTopic> 
                <robotBaseFrame>base_footprint</robotBaseFrame>
            </plugin>
        </gazebo> 

~~~~~~~~~~~~~~~~~~~~~~~~
◆robotNamespace：Robotが複数ある場合はNameで分けられる
◆leftJoint,rightJoint：左右車輪のJointを指定する、これでモデルと駆動輪をリンクすることができる
◆commandTopic：速度指令Topic
◆wheelSeparation：ホイールペース
◆wheelDiameter：ホイール直径
◆odometryFrame：座標情報

https://qiita.com/ryuka0610/items/5a5df638543abfa56dce

ステップ２(シミュレーション環境構築)
=
ロジック、ソフトを検証するため、ロボットのテストするための実世界を模擬した空間をGazebo上に作る。
作り方は主に２つある

■方法１．Gazebo上でダウンロードしたモデルを直接挿入する、これはCafeを入れて見た様子、更にこの中に色んな家具を入れられる。
![Screenshot from 2023-07-02 19-30-29.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/c7b62a95-0251-5b49-3343-b15b9a9e60a8.png)

環境構築終わったら、File→Save World as を使って環境を保存できる。保存した環境はLaunchファイルの中world設定すれば、毎回モデルと一緒に使えるわけ。
~~~~~~~~~~~~~~~~~~~~~~~~
 <arg name="world_name" value="$(find mrobot_gazebo)/worlds/playground.world"/>
~~~~~~~~~~~~~~~~~~~~~~~~

■方法２．Gazebo上のBuild Editorにて直接環境を編集する
メニューのEdit→Build Editor
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3181208/0129b092-ec57-bb4f-fd1b-934b4cf28658.png)
編集したモデルはモデルファイルとして保存すれば、次回もそのまま使える。


ステップ３(シミュレーション)
=
環境整備終わったら、最後launchファイルを作り、シミュレーションする。
このlaunchファイルは主にロボットモデル、world環境指定、gazebo表示設定、後は定番のJoint_state_publisherとrobot_state_publisherを入れているくらい。

シミュレーションモデルlaunch
~~~~~~~~~~~~~~~~~~~~~~~~
<launch>

    <!-- launchパラメータ、環境worldを指定する -->
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/playground.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- gazebo環境起動設定 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <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>

    <!-- Robot本体の起動 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/mrobot.urdf.xacro'" /> 

    <!-- Joint_state_publisherにてJoint状態を発表  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 

    <!-- robot_state_publisherにてtfを発表  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- gazebo内Robotモデルをloadする-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mrobot -param robot_description"/> 

</launch>
~~~~~~~~~~~~~~~~~~~~~~~~

シミュレーションモデルlaunch起動後、別のターミナルでmrobot_teleop.launchを起動すれば、キーボードでロボットモデルの操作ができるようになる。
teleop方向、速度指令→cmd_vel→odom→gazeboという流れで最終的robotがgazebo上で移動し、gazebo上で物理的なシミュレーションを実現するイメージ。