rvizを使いこなす！

[参考サイト](https://qiita.com/srs/items/5f44440afea0eb616b4a#%E3%82%B7%E3%83%9F%E3%83%A5%E3%83%AC%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%E7%B7%A81gazebo%E5%9F%BA%E7%A4%8E)をもとに進めた．

[1. URDFファイルを記述してロボットのlinkやjointを定義する](#1-URDFファイルを記述してロボットのlinkやjointを定義する)
[2. rviz上のロボットを動かすノードを作成](#2-rviz上のロボットを動かすノードを作成)
[3. launchファイルを記述してrvizと一緒にほかのノードを起動する](#3-launchファイルを記述してrvizと一緒にほかのノードを起動する)
[4. 複数台の（同じ）ロボットを表示する](#4-複数台の（同じ）ロボットを表示する)
[5. 複数台の異なるロボットを表示する](#5-複数台の（異なる）ロボットを表示する)

# 1. URDFの記述
[1-1. 前準備](#1-1-前準備)
[1-2. URDFの作成①](#1-2-URDFの作成①)
[1-3. URDFの作成②](#1-3-URDFの作成②)
[1-4. 駆動させる（多関節ロボット）](#1-4-駆動させる（多関節ロボット）)
    [※jointの種類について](#-jointの種類について)

## URDFとは
URDF (Unified Robot Description Format)とは，ロボットの構造を記述するためのXMLのフォーマット．URDFはLinkとJointから構成される．
URDFに詳細なオプションを記述することでsimulation用モデルを作成することが出来る．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/f8c5fe1c-ad3f-a4bc-c3c7-2d84f6a0772d.png)


### 1-1. 前準備
	1. パッケージ(vis_lecture)の作成
		$ cd ~/practice_ws/src/
    	$ catkin_create_pkg vis_lecture std_msgs rospy roscpp tf sensor_msgs
		
	2. URDFチュートリアルパッケージのインストール(*melodic*部分は自分のrosのバージョン名に変更)
       この中に含まれているURDFを表示するパッケージを利用する
	
    	$ sudo apt install -y ros-*melodic*-urdf-tutorial 
	
	3. URDFコードをチェックするパッケージのインストール
	
    	$ sudo apt install liburdfdom-tools

### 1-2. URDFの作成②
1. URDFを記述するファイルを作成
   URDFはパッケージファイル直下のURDFファイルに作成するのが慣例．それに従い，「simple_body1.urdf」を作成する
	```
	$ vim /vis_lecture/urdf/simple_body1.urdf
	※存在しないフォルダなどは☝のコマンドで新規作成される
	```

2. 簡単な箱を表示するURDFを作成
    	
    ```cpp:simple_body1.urdf
    <robot name="test_robot">
        <link name="base_link"/>
    
        <joint name="body_joint" type="fixed">
            <parent link="base_link"/>
            <child  link="body_link"/>
        </joint>
        <link name="body_link">
        	<visual>
        	    <geometry>
        	        <box size="0.3 0.3 0.2"/>
        	    </geometry>
        	    <origin xyz="0 0 0" rpy="0 0 0"/>
        	    <material name="red">
        	        <color rgba="1.0 0.0 0.0 1.0"/>
        	    </material>
        	</visual>
        </link>
    </robot>
    ```
	このコードによって，☟のような構成のモデルが作られる．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/0688ece1-6ea2-8b88-0b88-a018fac85e6a.png)


3. 記述したURDFに間違いがないかチェックする．ただし，このツールはlinkとjointの関係のみチェックするので，色や形などの記述を間違えていてもチェックしてくれない．
	```
	$ cd ~/practice_ws/src/vis_lecture/urdf
	$ check_urdf simple_body1.urdf
	```
	
	正しくかけていれば，☟のように表示される
	![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/e622b7c1-e75a-2699-9a46-0c0e568d3f67.png)

4. URDFの表示
	本当はURDFの作成に加えて，Rviz上のロボットを動かすためのROSノードやlaunchファイルの記述が必要だが，とりあえず表示してみる．
	```
	$ roscd vis_lecture/urdf
	$ roslaunch urdf_tutorial display.launch model:=simple_body1.urdf
	```
	![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/60088552-e068-ca1e-966f-208926e8b693.png)

	Rvizが起動し，☝のように表示されればOK

### 1-3. URDFの作成②
色々な形状・色のモデルを表示する
1. URDFを記述するファイルを作成
    ```
    $ vim /vis_lecture/urdf/simple_body2.urdf
    ※存在しないフォルダなどは☝のコマンドで新規作成される
    ```

2. URDFを作成
    ☟のような構成のモデルを表示する
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/9c9e727b-6cb1-8817-9400-83d74478b362.png)

    ```cpp:simple_body2.urdf
    <robot name="test_robot">
        <link name="base_link"/>
        
        <joint name="body1_joint" type="fixed">
            <parent link="base_link"/>
            <child  link="body1_link"/>
        </joint>
        
        <link name="body1_link">
            <visual>
                <geometry>
                    <box size="0.3 0.3 0.2"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <material name="red">
                    <color rgba="1.0 0.0 0.0 2.0"/>
                </material>
            </visual>
        </link>
        
        <joint name="body2_joint" type="fixed">
            <parent link="base_link"/>
            <child  link="body2_link"/>
            <origin xyz="0.5 0 0" rpy="0 0 0"/>
        </joint>
        
        <link name="body2_link">
            <visual>
                <geometry>
                    <sphere radius="0.2"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <material name="green">
                    <color rgba="0.0 1.0 0.0 2.0"/>
                </material>
            </visual>
        </link>
        
        <joint name="body3_joint" type="fixed">
            <parent link="base_link"/>
            <child  link="body3_link"/>
            <origin xyz="-0.5 0 0" rpy="0 0 0"/>
        </joint>
        
        <link name="body3_link">
            <visual>
                <geometry>
                <cylinder length="0.3" radius="0.1"/>
                </geometry>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <material name="blue">
                    <color rgba="0.0 0.0 1.0 2.0"/>
                </material>
            </visual>
        </link>
    </robot>
    ```
3. URDFのチェックと表示
    ```
    $ check_urdf simple_body2.urdf
    $ roslaunch urdf_tutorial display.launch model:=simple_body2.urdf
    ```

    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/7120942a-33bb-8ba9-a994-bbabce677b56.png)
    ☝のように表示されればOK


### 1-4. 駆動させる（多関節ロボット）
1. URDFを記述するファイルの作成
	```
	$ vim /vis_lecture/urdf/simple_body3.urdf
	```
2. 多関節ロボットのURDF
    <font color="Red">※launchファイルを作成する前にjointを駆動にするとエラーが発生する．そこで，はじめは"revolute"を"fixed"に置き換えるとエラー無く表示できる．</font>
	```cpp:simple_bosy3.urdf
	<robot name="test_robot">
	  <material name="red">
	    <color rgba="1.0 0.0 0.0 2.0"/>
	  </material>
	
	  <link name="base_link"/>
		
	  <joint name="body1_joint" type="fixed">
	    <parent link="base_link"/>
	    <child  link="body1_link"/>
	  </joint>
	
	  <link name="body1_link">
	    <visual>
	      <geometry>
	        <box size="0.1 0.1 0.5"/>
	      </geometry>
	      <origin xyz="0 0 0.25" rpy="0 0 0"/>    ///<==link/jointの原点位置（一つ前のjointの相対座標になってる）
	      <material name="red"/>
	    </visual>
	  </link>
		
	  <joint name="body2_joint" type="revolute">      ///<== revolute=回転する
	    <parent link="body1_link"/>
	    <child  link="body2_link"/>
	    <origin xyz="0.1 0 0.5" rpy="0 0 0"/>
	    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>     ///<== limitの値はrad表記で必ず記述する
	  </joint>
		
	  <link name="body2_link">
	    <visual>
	      <geometry>
	        <box size="0.1 0.1 0.4"/>
	      </geometry>
	      <origin xyz="0 0 0.2" rpy="0 0 0"/>
	      <material name="red"/>
	    </visual>
	  </link>
	
	  <joint name="body3_joint" type="revolute">
	    <parent link="body2_link"/>
	    <child  link="body3_link"/>
	    <origin xyz="0.1 0 0.4" rpy="0 0 0"/>
	    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
	  </joint>

	  <link name="body3_link">
	    <visual>
	      <geometry>
	        <box size="0.1 0.1 0.4"/>
	      </geometry>
	      <origin xyz="0 0 0.2" rpy="0 0 0"/>
	      <material name="red"/>
	    </visual>
	  </link>
	</robot>
	```
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/78214b87-866b-d552-9e8d-818a3053ce93.png)
    ☝このような構成
3. テストをしてURDFを表示する
    ```
	$ check_urdf simple_body3.urdf
	$ roslaunch urdf_tutorial display.launch model:=simple_body4.urdf
    ```
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/bd0b96c1-5490-96ba-b656-e9dc467b5831.png)
    ☝こんな感じ


### jointの種類について
    
<table>
    <caption>jointの種類</caption>
    <thread>
        <th>joint type</th> <th>意味</th>
    </thread>
    <tr>
        <td>fixed</td> <td>	動けないジョイント． 子供リンクが親リンクに固定されている場合に利用</td>
    </tr>
    <tr>
        <td>continuous</td> <td>回転可能なリンク．ただし， 回転数や角度に制限はない． 車輪によく利用される．</td>
    </tr>
    <tr>
        <td>revolute</td> <td>回転可能なリンク．ただし， 角度に制限がある． マニピュレータのジョイントとしてよく利用される．</td>
    </tr>
    <tr>
        <td>prismatic</td> <td>滑るジョイント． ジョイントの舳に１次元で滑ることが出来る． 別名「linear joint」．ただし，滑れる範囲に制限がある．</td>
    </tr>
    <tr>
        <td>floating</td> <td>６自由度のジョイント． 子供リンクは自由に動くことが出来る．</td>
    </tr>
    <tr>
        <td>planar</td> <td>平面に動けるジョイント．CNC機のように３自由度を持つ部分に利用する．</td>
    </tr>
</table>

## 2. rviz上のロボットを動かすノードを作成
　rvizはROSで動作するロボットの現状を表示するツールなので，駆動状況なども表示できる．その仕組みは下図のようになっている．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/1fc71299-60f2-7d23-8cd2-cf98e5f029e5.png)
　駆動部分が1軸の関節か，2軸以上の関節かで/robot_state_publisherを経由するかどうかが変わるが，基本的にはuser_nodeから/tfに「どの関節が」「どれだけ動くか」をpublishし，rvizがそれを読み取ることで状態を表示する仕組みである．
/tf_staticでは駆動しない関節の，/tfでは駆動する関節の情報をpublishする．

1. 先ほどrvizで表示させる際に，エラー対策として駆動部分を"revolute"→"fixed"に変更した場合は，URDFの記述を"revolute"に戻す．
2. srcフォルダ下に移動して，ソースファイルを作成
    ```		
    $ roscd vis_lecture/src
    $ vim joint_publisher.cpp
    ```	
3. コードは以下のように記述する
    ```cpp:joint_publisher.cpp	
    #include <ros/ros.h>     //ROSの基本ヘッダーファイル
    #include <sensor_msgs/JointState.h>　　
    //joint_statesにpublishするためのヘッダーファイル
    
    #include <string>
    #include <math.h>　//以下のコード内で計算などを行うためのパッケージ
    
    int main(int argc, char** argv)
    {
      ros::init(argc, argv, "vis_joint_publisher");　
        //ノード名を"vis_joint_publisher"に初期化
      ros::NodeHandle nh;　　 //ROSシステムと通信を行うための宣言
      ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
      //☝では，ROSに装備されているsensor_msgsパッケージのJointStateメッセージファイルを用いて，"joint_pub"という名前のpublisherを作成．このとき，topic名は"joint_states"として，publisher queueのサイズを10に設定．
    
      ros::Rate loop_rate(10);　
        //ループ周期を設定．10Hz（0.1秒間隔）での繰り返しにする
      
      int count = 0;　
        //publishするメッセージ内容で数字を扱うために，変数を宣言
      while (ros::ok())
      {
        sensor_msgs::JointState js0;　
        //sensor_msgs::JointStateという言葉をjs0で省略表記する宣言
        js0.header.stamp = ros::Time::now();　
        //現在時間をヘッダーファイルに載せる
        js0.name.resize(2);
        //nameを２つ要素を持つ変数に変更→jointの名前を入力する
        js0.name[0] = "body2_joint";
        js0.name[1] = "body3_joint";
        js0.position.resize(2);　
        //positionを2つの要素を持つ変数に変更→jointの角度情報を入力する
        js0.position[0] = -1.0 * (float)count / 40.0;
        js0.position[1] = 2.0 * (float)count / 40.0;
    
        joint_pub.publish(js0);　//js0の内容をメッセージとしてpublishする
        count++;　//カウント変数を１ずつ増やす
    
        ros::spinOnce();
        loop_rate.sleep();　
        //設定したループ周期（10Hz）に合わせてスリープ（待機）を実行
      }
      return 0;
    		}
    ```
		
	参考サイト：	http://wiki.ros.org/sensor_msgs
		https://qiita.com/aquahika/items/6e3753d4498a190bddb1


## 3. launchファイルを記述してrvizと一緒にほかのノードを起動する

1. launchファイルの作成
	URDFファイルとpublisherノードファイル，これらをrvizとともに立ち上げるためにはlaunchファイルを記述する必要がある．
    ```
	$ roscd vis_lecture
	$ mkdir launch
	$ roscd launch
	$ vim joint_display.launch
    ```
	コードは以下のように記述する
	```cpp:joint_display.launch
    	<launch>
    	//arg：launchファイル内での変数の定義するタグ
    	  <arg name="model" default="$(find vis_lecture)/urdf/simple_body3.urdf"/>
    	//rvizに表示するmodelのURDFファイル場所を変数に格納
    	  <arg name="rvizconfig" default="$(find vis_lecture)/urdf.rviz">
    	//rvizを起動するためのurdf.rvizのパスを変数に格納
    	  
    	//node：ノードの起動・実行について記述するタグ．パッケージ名・ノード名・実行名などを記述する
    	  <node name="vis_joint_publisher" pkg="vis_lecture" type="vis_joint_publisher"/>
    	//vis_joint_publisherで関節角度をpublish
    	  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    	//robot_state_publisherで関節角度をsubscribe＆rvizにpublish
    	  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
    	//rvizの設定ファイルをもとにノードを作成
    	    	
    	//param：パラメータ名，タイプ，値などを設定するタグ
    	  <param name="robot_description" textfile="$(arg model)" />
    	</launch>
    ```

2. CMakeListの編集
	自動で作成されるが，大部分はコメントアウトされているため，用途に合わせて編集する必要がある．（コメントアウトを外したり，追記したり）
	```
	$ roscd vis_lecture
	$ vim CmakeList.txt
	```

    ```cpp:CmakeList.txt
	<CMakeList>
	 cmake_minimum_required(VERSION 3.0.2)                                                                                                                
	 project(vis_lecture) 
    //ビルドに必要な依存パッケージ一覧
	find_package(catkin REQUIRED COMPONENTS
	  roscpp
	  rospy
	  std_msgs
	  tf
	  sensor_msgs
	)
    //依存するメッセージパッケージを設定
	generate_messages(
	  DEPENDENCIES
	  std_msgs
	  sensor_msgs
	)
    //catkinビルド時のオプションを記載（パッケージ名や依存パッケージなど）
	catkin_package(
	 INCLUDE_DIRS include
	 LIBRARIES vis_lecture
	 CATKIN_DEPENDS roscpp rospy std_msgs tf sensor_msgs
	 DEPENDS system_lib
	)
	include_directories(
	  ${catkin_INCLUDE_DIRS}
	)

    //ビルド後作成する実行ファイル（ノード）とそれを作るのに参照するソースコードファイル
	add_executable(vis_joint_publisher src/joint_publisher.cpp)
	
	add_dependencies(
	　　vis_joint_publisher 　${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
	
    //実行ファイルの依存ファイルと作成するのに必要なリンク
	target_link_libraries(vis_joint_publisher
	  ${catkin_LIBRARIES}
	)
	
3. urdf_tutorialで使われているrvizの設定ファイルをvis_lectureパッケージ内にコピーする．
※この設定ファイルがないと，うまくrvizが起動せず☟のように表示されてしまう（urdfで記述したモデルは表示されない）
	![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/d21148a1-ba48-b112-4838-ee2475faa74b.png)

	```
	$ roscd vis_lecture
	$ roscp urdf_tutorial urdf.rviz ./
	```

4. 作成したノードのビルド
	```
	$ cd ~/practice_ws
	$ cakin_make
	```
5. 実行
	```
	$ roslaunch vis_lecture joint_display.launch
	```
	![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/3df1cbea-8d95-bc7c-133b-4f1c1af543a4.png)

# 4. 複数台の（同じ）ロボットを表示する
一度に（descriptionファイルが）同じロボットを複数台表示するためには，何をどうすればいいのか．
gazeboを起動しない場合（実機と併用する場合）の解説が少なかったので，その方法をここにまとめる．
1. [下準備](#下準備)
1. [launchファイルの作成](#launchファイルの作成)
1. [rvizの諸設定](#rvizの諸設定)

## 下準備
まず，複数台表示のためにはrvizの`tf_prefix`と`joint_states`の扱いが重要になってくる．
* `tf_prefix`：同じロボットモデルの中で，区別するためのラベルのようなもの
* `remap joint_states`：ロボットの位置情報をpublishするトピック名を，ロボットごとに区別するために変更するコマンド．（変えなくても動くけど，使い勝手が悪い）
```xml:vis_lecture/launch/single_robot.launch
<launch>
    <arg name="robot_name" />
    <arg name="model" />

    <!-- Load robot model & show it on rviz -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)"/>
    <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hauser_viz)/launch/config/rviz/model_rviz.rviz" required="true" /> -->

    <!-- Publish tf info-->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
        <param name="tf_prefix" value="$(arg robot_name)" />
        <remap from="joint_states" to="/$(arg robot_name)/joint_states" />
    </node>
</launch>
```
ロボット一つ当たりの表示に必要な情報は☝のlaunchファイルに記載されているので，さらにこれをincludeして名前をそれぞれ付けることで複数台表示できるようにする．

## launchファイルの作成
```xml:vis_lecture/launch/multi_robot.launch
<launch>
    <group ns = "master">
        <include file="$(find vis_lecture)/launch/single_robot.launch">
            <arg name="robot_name" value="master" />
            <arg name="model" value="$(find vis_lecture)/urdf/simple_body3.urdf" />
        </include>
    </group>

    <group ns = "slave">
        <include file="$(find vis_lecture)/launch/single_robot.launch">
            <arg name="robot_name" value="slave" />
            <arg name="model" value="$(find vis_lecture)/urdf/simple_body3.urdf" />
        </include>
    </group>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_description)/config/urdf.rviz" required="true" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="base_links_tf" args="0 2 0 0 0 0 master/base_link slave/base_link" />
</launch>
```
☝で同種ロボットを複数台表示できるようになる．
* `group`タグ：`master`・`slave`と名付けた2つの同種のロボットを表示する．
* `rviz`：rvizを起動
* `tf2_ros`：`master`と`slave`の`base_link`間の相対距離を設定する．これを書かないと，rvizで原点に固定されているロボット以外は，場所が分からないので表示されない．


## rvizの諸設定
rviz側のGUI操作も少々癖があるので注意が必要．

まずは，先ほど作成したlaunchファイルを用いてrvizを起動する．
```
$ roslaunch vis_lecture multi_robot.launch
```
1. `Fixed Frame`は`master/base_link`に設定（これまでとほぼ同じ）
1. 下のAddボタンから，`RobotModel`を**2つ**追加
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/f94349fb-c830-08f4-5427-7f269e2d9d17.png)

1. `RobotModel`のうち，`Robot Description`と`TF Prefix`を以下のように編集する．ただし，`group`タグを利用しない場合は`TF Prefix`のみ変更すればいい．
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/bf9e051e-f548-7a53-bcd9-083bfcd6ba14.png)

# 5. 複数台の（異なる）ロボットを表示する
異なるロボットを表示する場合も，基本的には同じロボットを表示するときと同じ．
ただし，`xacro`ファイルが異なるので，`model`に入れるパスを変更する必要がある．（`tf2_ros`の相対位置設定で，link名も変更する必要があるかも）

```xml:vis_lecture/launch/multi_robot.launch
<launch>
    <group ns = "master">
        <include file="$(find vis_lecture)/launch/single_robot.launch">
            <arg name="robot_name" value="master" />
            //////// Set Model Pass ////////
            <arg name="model" value="$(find vis_lecture)/urdf/simple_body1.urdf" />
        </include>
    </group>

    <group ns = "slave">
        <include file="$(find vis_lecture)/launch/single_robot.launch">
            <arg name="robot_name" value="slave" />
            //////// Set Model Pass ////////
            <arg name="model" value="$(find vis_lecture)/urdf/simple_body3.urdf" />
        </include>
    </group>

    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_description)/config/urdf.rviz" required="true" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="base_links_tf" args="0 2 0 0 0 0 master/base_link slave/base_link" />
</launch>
```