ROS講座24 RvisにVisualizationMarkerを表示する

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

| 項目 | 値 |
|:-:|:-:|
| 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)を参照してください。

#概要

Rvizの画面上に以下のようにマーカーを置くことができます。物体を置けるという点ではurdと似ていますが、VisualizationMarkerのほうがROSトピック経由で手軽にプログラムから形を指示出来て、またRviz上でのチェックで表示の切り替えを個別に行うこともできます。

![marker.png](https://qiita-image-store.s3.amazonaws.com/0/254442/97e2e702-228f-a58d-39e8-0cf19c2a4050.png)
画像はROSWikiより

# 例1（直方体を出す）

## プログラム

```c++:info_lecture/src/info_marker_publisher1.cpp
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <string>
#include <math.h>

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

  // publisher
  ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("marker", 1);

  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    marker.header.frame_id = "world";
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.id = 0;

    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration();

    marker.scale.x = 0.5;
    marker.scale.y = 0.5;
    marker.scale.z = 0.2;
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0;
    marker.pose.orientation.y = 0;
    marker.pose.orientation.z = 0;
    marker.pose.orientation.w = 1;
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0f;
    marker_pub.publish(marker);

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

Markerは設定項目が多いのですが、難しい項目はありません

* header系
    * header.frame_id：位置の基準となるtfのフレームを指定します。      
例えばworldフレームの中の定位置にmarkerを出したいのかなのか、常にロボットの正面に出現させたいのかをこれで指定します。
    * header.stamp：多分設定しなくてもよさそうな感じがしていますが、現在時刻「ros::Time()」を入れておきます。
    * marker.ns, marker.id：マーカーの名前を指定します。visualization markerは後で消したり、上書きができる仕様になっています。nsとidの両方が一致していると同じものと判定されます。
* 種類系
    * marker.type：表示するmarkerの種類を指定します。詳しくは参考のROSwikiを見てください。
    * marker.action：markerへの操作を指定します。とりあえずvisualization_msgs::Marker::ADDを指定しておけば問題ありません。
    * lifetime：markerの表示を持続する時間です。とりあえず無限「ros::Duration()」を指定すれば問題ありません。
* 位置、大きさ、色（以下はtypeがCUBE、SPHERE、CYLINDERの場合の説明です）
    * scale：大きさを指定します。
    * pose.position：位置を指定します。
    * pose.orientation：姿勢を指定します。(x,y,z,w)=(0,0,0,1)が回転なしです。
    * color：色を指定します。color.aは透明率で、0は完全に透明（見えない）、1が不透明です。

以下にこれらのノードを起動するためのlaunchを載せます。

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

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

## 実行&Rvizの設定
roslaunchを起動しましょう。各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```
roslaunch info_lecture marker1.launch
```

ここで`rostopic echo /marker`とするとVisualizationMarkerのROSトピックがpublishされているのが見えます。

```text
header: 
  seq: 204
  stamp: 
    secs: 1589013229
    nsecs: 920559500
  frame_id: "world"
ns: "basic_shapes"
id: 0
type: 1
action: 0
pose: 
  position: 
    x: 0.0
    y: 0.0
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
scale: 
  x: 0.5
  y: 0.5
  z: 0.2
color: 
  r: 0.0
  g: 1.0
  b: 0.0
  a: 1.0
lifetime: 
  secs: 0
  nsecs:         0
frame_locked: False
points: []
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False
```

CLI上の表示だけでは何なのかわかりずらいのでRviz上での表示を見てみましょう。
もしros_lectureのgitリポジトリを使用している場合はrvizの表示設定が読み込まれるのでmarkerが表示されますが、そうでない場合はRvizの設定が必要です。

まずRvizでvisualization markerを追加します。画面左下の「Add」を押して、出てくる画面で「Marker」を選択します。

![marker2.png](https://qiita-image-store.s3.amazonaws.com/0/254442/2602b5c7-b9c6-8671-357d-446473df53ba.png)

Fixed Frameをworldに設定、Marker Topicにvisualization markerをpublishしているtopic名の「/marker」を入れます。

![marker3.png](https://qiita-image-store.s3.amazonaws.com/0/254442/440a9c6e-759e-23c6-ed09-7fd90a1ec68c.png)

見事にmarkerを表示することができました。

# 例2（矢印を動的に表示）
次に複数のmarkerの表示をしてみます。上記の例1で使用したMarkerを複数回publishすることでも実現できますが、あまり数が多すぎるとこの方法ではrvizの制約で正しくで表示されないことがあります。visualization_msgs::Markerの代わりにvisualization_msgs::MarkerArrayを使ってみます。これを使うと数多くのMarkerを扱うことができます。
MarkerArrayはその名の通り、Markerが配列になっているものです。複数のマーカーを1つのpublisherで作成できます。
今回は動的に表示が変わるMarkerを作ります。前に作った移動ロボットのシミュレーションで速度指示を可視化してみます。

## プログラム
```c++:info_lecture/src/info_marker_publisher2.cpp
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/MarkerArray.h>

#include <string>
#include <math.h>

geometry_msgs::Twist twist_last;
void twist_callback(const geometry_msgs::Twist& twist_msg)
{
  twist_last = twist_msg;
}

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

  // publisher
  ros::Publisher marker_pub = nh.advertise<visualization_msgs::MarkerArray>("marker_array", 1);

  // subscriber
  ros::Subscriber twist_sub = nh.subscribe("/cmd_vel", 10, twist_callback);

  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    float length = sqrt(pow(twist_last.linear.x, 2) + pow(twist_last.linear.y, 2));
    float angle = atan2(twist_last.linear.y, twist_last.linear.x);
    geometry_msgs::Point linear_start;
    linear_start.x = 0.3 * cos(angle);
    linear_start.y = 0.3 * sin(angle);
    linear_start.z = 0;
    geometry_msgs::Point linear_end;
    linear_end.x = (0.3 + length) * cos(angle);
    linear_end.y = (0.3 + length) * sin(angle);
    linear_end.z = 0;

    geometry_msgs::Point angular_start;
    angular_start.x = 0.3;
    angular_start.y = 0;
    angular_start.z = 0;

    geometry_msgs::Point angular_end;
    angular_end.x = 0.3;
    angular_end.y = 0 + 0.3 * twist_last.angular.z;
    angular_end.z = 0;

    geometry_msgs::Vector3 arrow;  // config arrow shape
    arrow.x = 0.02;
    arrow.y = 0.04;
    arrow.z = 0.1;

    visualization_msgs::MarkerArray marker_array;
    marker_array.markers.resize(2);

    // marker0
    marker_array.markers[0].header.frame_id = "/base_link";
    marker_array.markers[0].header.stamp = ros::Time::now();
    marker_array.markers[0].ns = "cmd_vel_display";
    marker_array.markers[0].id = 0;
    marker_array.markers[0].lifetime = ros::Duration();

    marker_array.markers[0].type = visualization_msgs::Marker::ARROW;
    marker_array.markers[0].action = visualization_msgs::Marker::ADD;
    marker_array.markers[0].scale = arrow;

    marker_array.markers[0].points.resize(2);
    marker_array.markers[0].points[0] = linear_start;
    marker_array.markers[0].points[1] = linear_end;

    marker_array.markers[0].color.r = 0.0f;
    marker_array.markers[0].color.g = 1.0f;
    marker_array.markers[0].color.b = 0.0f;
    marker_array.markers[0].color.a = 1.0f;

    // marker1
    marker_array.markers[1].header.frame_id = "/base_link";
    marker_array.markers[1].header.stamp = ros::Time::now();
    marker_array.markers[1].ns = "cmd_vel_display";
    marker_array.markers[1].id = 1;
    marker_array.markers[1].lifetime = ros::Duration();

    marker_array.markers[1].type = visualization_msgs::Marker::ARROW;
    marker_array.markers[1].action = visualization_msgs::Marker::ADD;
    marker_array.markers[1].scale = arrow;

    marker_array.markers[1].points.resize(2);
    marker_array.markers[1].points[0] = angular_start;
    marker_array.markers[1].points[1] = angular_end;

    marker_array.markers[1].color.r = 1.0f;
    marker_array.markers[1].color.g = 0.0f;
    marker_array.markers[1].color.b = 0.0f;
    marker_array.markers[1].color.a = 1.0f;

    marker_pub.publish(marker_array);

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

* MarkerArrayはただMarkerが配列になっているだけのものです。`visualization_msgs::MarkerArray marker_array`で変数を宣言した後に`marker_array.markers.resize(2)`として配のサイズを変更します。この辺りはstdコンテナそのままです。こうすることで`visualization_msgs::Marker`型の変数`marker_array.markers[0]`と`marker_array.markers[1]`が使えるようになります。
* `marker_array.markers[0]`ではtwistのlinear.x、linear.y成分を緑の矢印で、`marker_array.markers[1]`ではtwistのangular.z成分を赤の矢印で表示します。
* 今回のmarkerでは矢印を表示します。矢印の表示の仕方は2種類あるのですが、今回はそのうちの始点と終点を指定する方式を使います。始点と終点の2点を指定するために`marker_array.markers[0].points.resize(2)`でpoint[0]とpoint[1]を作ります。それぞれが始点と終点の座標を表すPoint型の変数になります。
* 矢印の大きさは`scale`のフィールドで指定します。矢印の場合はちょっと変わっていて以下のことを表しています。
    * scale.xで矢印の軸の直径
    * scale.xで矢印の傘の底面の直径
    * scale.xで矢印の傘の高さ
* またVisualizationMarkerは`ns`と`id`で識別されています。`markers[0]`と`markers[1]`でこの2つのフィールドが同一だと上書きしてしまいます。


```xml:info_lecture/launch/marker2.launch
<launch>
  <arg name="model" default="$(find vis_lecture)/urdf/move_robot.urdf" />
  <arg name="rvizconfig" default="$(find info_lecture)/rviz/marker2.rviz" />
  <arg name="joy" default="false" />  
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  <group if="$(arg joy)">
    <node name="joy_node"   pkg="joy" type="joy_node" />
    <node name="twist_publisher" pkg="basic_lecture" type="basic_twist_publisher"/>
  </group>
  <node name="info_marker_publisher2" pkg="info_lecture" type="info_marker_publisher2" />
  <node name="vis_robot_sim" pkg="vis_lecture" type="vis_robot_sim"/>

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

```

## 実行&Rvizの設定

ジョイスティックをつないでください。各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roslaunch info_lecture marker2.launch joy:=true
```



もしros_lectureのgitリポジトリを使用している場合はrvizの表示設定が読み込まれるのでmarkerが表示されますが、そうでない場合はRvizの設定が必要です。

ロボットの表示方法はROS講座18を見てください。

次にmarker_arrayをまずRvizでvisualization markerを追加します。画面左下の「Add」を押して、出てくる画面で「MarkerArray」を選択します。
Fixed Frameをworldになっていることを確認。またMarker Topicにvisualization marker_arrayをpublishしているtopic名の「/marker_array」を入れます。

![marker4.png](https://qiita-image-store.s3.amazonaws.com/0/254442/20efa74c-86e0-22b5-f7c8-585dd2bab528.png)

これによって見事にmarkerを表示することができました。joyで操作している画面を張っておきます。

![vis_move2.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/114b84ff-718b-2773-50a6-bd1f2fb3e813.gif)


# 参考
[ROSwiki Visualization marker](http://wiki.ros.org/rviz/DisplayTypes/Marker)

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