rvizを使いこなす！

参考サイトをもとに進めた．

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

1. URDFの記述

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

URDFとは

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

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を作成

   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>
   

   このコードによって，☟のような構成のモデルが作られる．
   

3. 記述したURDFに間違いがないかチェックする．ただし，このツールはlinkとjointの関係のみチェックするので，色や形などの記述を間違えていてもチェックしてくれない．

   $ cd ~/practice_ws/src/vis_lecture/urdf
   $ check_urdf simple_body1.urdf
   

   正しくかけていれば，☟のように表示される
   

4. URDFの表示
   本当はURDFの作成に加えて，Rviz上のロボットを動かすためのROSノードやlaunchファイルの記述が必要だが，とりあえず表示してみる．

   $ roscd vis_lecture/urdf
   $ roslaunch urdf_tutorial display.launch model:=simple_body1.urdf
   

   

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

1-3. URDFの作成②

色々な形状・色のモデルを表示する

1. URDFを記述するファイルを作成

   $ vim /vis_lecture/urdf/simple_body2.urdf
   ※存在しないフォルダなどは☝のコマンドで新規作成される
   

2. URDFを作成
   ☟のような構成のモデルを表示する
   

   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
   

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

1-4. 駆動させる（多関節ロボット）

1. URDFを記述するファイルの作成

   $ vim /vis_lecture/urdf/simple_body3.urdf
   

2. 多関節ロボットのURDF
   ※launchファイルを作成する前にjointを駆動にするとエラーが発生する．そこで，はじめは"revolute"を"fixed"に置き換えるとエラー無く表示できる．
   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>
   

   
   ☝このような構成
3. テストをしてURDFを表示する

   $ check_urdf simple_body3.urdf
   $ roslaunch urdf_tutorial display.launch model:=simple_body4.urdf
   

   
   ☝こんな感じ

jointの種類について

jointの種類

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

2. rviz上のロボットを動かすノードを作成

　rvizはROSで動作するロボットの現状を表示するツールなので，駆動状況なども表示できる．その仕組みは下図のようになっている．

　駆動部分が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. コードは以下のように記述する

   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
   

   コードは以下のように記述する

   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
   

   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で記述したモデルは表示されない）
   

   $ roscd vis_lecture
   $ roscp urdf_tutorial urdf.rviz ./
   

4. 作成したノードのビルド

   $ cd ~/practice_ws
   $ cakin_make
   

5. 実行

   $ roslaunch vis_lecture joint_display.launch
   

   

4. 複数台の（同じ）ロボットを表示する

一度に（descriptionファイルが）同じロボットを複数台表示するためには，何をどうすればいいのか．
gazeboを起動しない場合（実機と併用する場合）の解説が少なかったので，その方法をここにまとめる．

1. 下準備
2. launchファイルの作成
3. rvizの諸設定

下準備

まず，複数台表示のためにはrvizのtf_prefixとjoint_statesの扱いが重要になってくる．

• tf_prefix：同じロボットモデルの中で，区別するためのラベルのようなもの
• remap joint_states：ロボットの位置情報をpublishするトピック名を，ロボットごとに区別するために変更するコマンド．（変えなくても動くけど，使い勝手が悪い）

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ファイルの作成

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に設定（これまでとほぼ同じ）

2. 下のAddボタンから，RobotModelを2つ追加
   

3. RobotModelのうち，Robot DescriptionとTF Prefixを以下のように編集する．ただし，groupタグを利用しない場合はTF Prefixのみ変更すればいい．
   

5. 複数台の（異なる）ロボットを表示する

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

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>