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

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

#概要

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

画像はROSWikiより

例1（直方体を出す）

プログラム

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を載せます。

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されているのが見えます。

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」を選択します。

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

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

例2（矢印を動的に表示）

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

プログラム

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つのフィールドが同一だと上書きしてしまいます。

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を実行する必要があります。

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」を入れます。

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

参考

ROSwiki Visualization marker

目次ページへのリンク

ROS講座の目次へのリンク