ROS講座23 jskプラグインでrviz上に情報を表示する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ROSでは情報がROSトピックとして流れているので`rostopic echo`をすることでデータの確認はできますが、ロボットの情報はCLI上ではなくGUIで確認しないとわかりずらいものが多々あります。基本的な型ならRvizのデフォルトのプラグインでRviz上の画面に表示できるのですが、細かいところに手が届いていないものも多々あります。jsk_rviz_pluginを使うとRviz上でいろいろな情報を表示することができます。

* floatの値を表示するPieChart、Plotter2D 
例えばバッテリー電圧やモーターを流れる電流など、監視したい`std_msgs::Float32`型の値をRviz上で表示します。

* 文字を表示するoverlay_menuとoverlay_text 
画面上に任意の文字や選択メニューを表示することができます。

* 3D画面中にマークを表示するpictgram、TFtrajectory 
3次元空間上の特定の位置にpictgramを表示したり、ロボットの移動した軌跡を表示したりできます。

# install
jskのrvizプラグラインをインストールします。

```shell:jsk_rvizプラグインのインストール
sudo apt install -y ros-noetic-jsk-visualization
```

# 例1（値の表示）

## ソースコード
本当はバッテリー電圧など意味のある値を見るものですが、サンプルとして適当な値を出力します。上記の画像のようなグラフを出力するためには`std_msgs::Float32`で値を出力します。

```c++:info_lecture/src/info_float32_publisher.cpp
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <math.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "info_float32_publisher");
  ros::NodeHandle nh;
  ros::Publisher float_pub = nh.advertise<std_msgs::Float32>("float32", 10);

  ros::Rate loop_rate(10);
  int count = 0;
  while (ros::ok())
  {
    std_msgs::Float32 float_data;
    float_data.data = sin(0.02 * count * 2 * M_PI);
    float_pub.publish(float_data);

    count++;
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

また以下のlaunchでrvizが起動できます。

```xml:info_lecture/launch/jsk_rviz1.launch
<launch>
  <arg name="rvizconfig" default="$(find info_lecture)/rviz/jsk_rviz1.rviz" />

  <node name="info_float32_publisher" pkg="info_lecture" type="info_float32_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

## 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roslaunch info_lecture jsk_rviz1.launch
```

![vis_overlay_plot.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/8ce9b4b5-363f-91ac-c886-e0b5587bd902.gif)

## Rvizの設定
もしros_lectureのgithubからcloneしている場合はRvizの設定ファイルが読まれるので自動で表示がされますが、そうでない場合は手動で表示設定をする必要があります。

### プラグインの起動
1. Rvizの左下の「Add」ボタンを押します。
2. 出てくる画面で「PieChart」または「Plotter2D」をクリックします。

![jsk_rviz3.png](https://qiita-image-store.s3.amazonaws.com/0/254442/0527f02a-2275-da6b-075d-9e7598ad4bf3.png)

### プラグインの詳細設定
思った通りの表示をするためには設定が必要です。主な設定を示します。

### Piechartの設定
* Topic: 表示したいstd_msg::Float32型のROSトピックを書きます。ドロップダウン形式で選択することもできます。
* Size: 表示する大きさをpixel単位で書きます。
* Left、top: 表示位置です。それぞれ0だとPieChartの左上がRvizの3D画面の左上と一致します。
* max value、min value: 円グラフの最大、最小の時の値を設定します。
グラフの下にプロットされている名前は設定の一番上の「PieChart」と書かれているところを選択して画面下の「Rename」で変えられます。

### Plotter2Dの設定
PieChartの差分のみ書きます。

* width、height: 表示の横、縦の大きさを書きます。

![jsk_rviz2.png](https://qiita-image-store.s3.amazonaws.com/0/254442/58bb238e-4c4d-cfb0-c711-c819f5f66a3d.png)

## 補足
これらのプラグインで表示できるのは`std_msgs::Flaot32`型のメッセージのみです。Flaot64やInt32ではできません。

#例2（メニュー表示・テキスト表示）

## ソースコード

### OverLayNenuをpublishするROSノード

```c++:info_lecture/src/info_overlay_menu_publisher.cpp
#include <ros/ros.h>
#include <jsk_rviz_plugins/OverlayMenu.h>
#include <string>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "info_overlay_menu_publisher");
  ros::NodeHandle nh;

  // publisher
  ros::Publisher menu_pub = nh.advertise<jsk_rviz_plugins::OverlayMenu>("menu", 1);

  ros::Rate loop_rate(2);
  bool flip = false;
  while (ros::ok())
  {
    jsk_rviz_plugins::OverlayMenu menu;
    menu.action = jsk_rviz_plugins::OverlayMenu::ACTION_SELECT;
    if (flip)
      menu.current_index = 0;
    else
      menu.current_index = 1;
    menu.menus.resize(2);
    menu.menus[0] = "item0";
    menu.menus[1] = "item1";
    menu.title = "Sample Menu";
    menu_pub.publish(menu);
    flip = !flip;
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

`current_index`はメニューのうちどの値に選択していることを示すマークをつけるかを制御します。

### OverLayTextをpublishするROSノード

```c++:info_lecture/src/info_overlay_text_publisher.cpp
#include <ros/ros.h>
#include <jsk_rviz_plugins/OverlayText.h>
#include <string>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "info_overlay_text_publisher");
  ros::NodeHandle nh;

  // publisher
  ros::Publisher text_pub = nh.advertise<jsk_rviz_plugins::OverlayText>("text", 1);

  ros::Rate loop_rate(2);
  while (ros::ok())
  {
    jsk_rviz_plugins::OverlayText text;
    text.action = jsk_rviz_plugins::OverlayText::ADD;
    text.width = 400;
    text.height = 100;
    text.left = 0;
    text.top = 0;

    std_msgs::ColorRGBA color1, color2;
    color1.r = 0;
    color1.g = 0;
    color1.b = 0;
    color1.a = 0.4;
    text.bg_color = color1;

    color2.r = 25.0 / 255;
    color2.g = 255.0 / 255;
    color2.b = 240.0 / 255;
    color2.a = 0.8;
    text.fg_color = color2;

    text.line_width = 1;
    text.text_size = 14;
    text.font = "Ubuntu";
    text.text = "hello";

    text_pub.publish(text);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

変数が多いので説明します。

* width, height, left, topの4つは表示する位置をしてする。Rvizの3D画面の左上が0で右、下方向が正になる。ただ後述するようにRvizの画面からでも指定できるので、値を入れなくてもよい。
* fg_color, bg_colorは文字色、背景色でstd_msgs/ColorRGBAで指定する。r,b,g,aは全て0~1の間で指定する。ただこれも後から指定できる。

### launchファイル

```xml:info_lecture/launch/jsk_rviz2.launch
<launch>
  <arg name="rvizconfig" default="$(find info_lecture)/rviz/jsk_rviz2.rviz" />

  <node name="info_overlay_menu_publisher" pkg="info_lecture" type="info_overlay_menu_publisher" />
  <node name="info_overlay_text_publisher" pkg="info_lecture" type="info_overlay_text_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

## 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roslaunch info_lecture jsk_rviz2.launch 
```

rvizの左下の「Add」で「OverlayMenu」と「OverlayText」を追加で表示する。それぞれtopicを/menu、/textに指定すれば表示されます。

![vis_jsk2_1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/a18df9ca-df88-36bc-8c86-f0f16ada2398.png)

# 例3（pictgram、軌跡を表示）

## ソースコード

### pictgramをpublishするROSノード

```c++:info_lecture/src/info_pictgram_publisher.cpp
#include <jsk_rviz_plugins/Pictogram.h>
#include <ros/ros.h>
#include <string>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "info_pictgram_publisher");
  ros::NodeHandle nh;

  // publisher
  ros::Publisher pictgram_pub = nh.advertise<jsk_rviz_plugins::Pictogram>("pictgram", 1);

  ros::Rate loop_rate(1);
  int type_count = 0;
  while (ros::ok())
  {
    jsk_rviz_plugins::Pictogram pictgram;
    pictgram.header.frame_id = "base_link";
    pictgram.header.stamp = ros::Time::now();
    pictgram.pose.position.z = 0.3;
    pictgram.pose.orientation.y = -0.71;
    pictgram.pose.orientation.w = 0.71;
    pictgram.action = jsk_rviz_plugins::Pictogram::ADD;
    pictgram.color.r = 1.0;
    pictgram.color.a = 1.0;

    if (type_count % 4 == 0)
    {
      pictgram.mode = jsk_rviz_plugins::Pictogram::PICTOGRAM_MODE;
      pictgram.character = "fa-angle-down";
      pictgram.size = 0.5;
    }
    else if (type_count % 4 == 1)
    {
      pictgram.mode = jsk_rviz_plugins::Pictogram::PICTOGRAM_MODE;
      pictgram.character = "tag";
      pictgram.size = 0.5;
    }
    else if (type_count % 4 == 2)
    {
      pictgram.mode = jsk_rviz_plugins::Pictogram::PICTOGRAM_MODE;
      pictgram.character = "down";
      pictgram.size = 0.5;
    }
    else
    {
      pictgram.mode = jsk_rviz_plugins::Pictogram::STRING_MODE;
      pictgram.character = "CHAR";
      pictgram.size = 0.2;
    }
    pictgram_pub.publish(pictgram);
    type_count++;
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

`fa-angle-down`、`tag`、`down`の3つのピクトグラムと`CHAR`という文字の4種類を順番に表示します。`pictgram.mode`に入れる値で`pictgram.character`がpictgram名なのか標示する文字列なのかが変わります。使えるピクトグラム名の一覧がなかなか見つからないのですが、`rosed jsk_rviz_plugins pictogram_all.py`で見れるサンプルの中に使える名前の一覧があります。また標準だとピクトグラムはxy平面に表示されるので、横から見ると全く分かりませんので、`orientation`に値を入れて90度傾ける必要があります。

## launch

```xml:info_lecture/launch/jsk_rviz3.launch
<launch>
  <arg name="rvizconfig" default="$(find info_lecture)/rviz/jsk_rviz3.rviz" />

  <node name="vis_tf_broadcast" pkg="vis_lecture" type="vis_tf_broadcast"/>
  <node name="info_pictgram_publisher" pkg="info_lecture" type="info_pictgram_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

jskプラグインではtfの軌跡を表示することが出来ます。過去に作ったtfを円形に動かすサンプルを流用します。

## 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roslaunch info_lecture jsk_rviz3.launch 
```

![vid_jsk3.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/27a92cb6-5cc4-8507-aa27-f9bfdc0e0c54.gif)


#参考
* [東大JSKが公開しているクールなROS可視化ライブラリを使ってみる](https://myenigma.hatenablog.com/entry/2015/10/30/223023)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)