LoginSignup
18
12

More than 1 year has passed since last update.

@srs(project_srs)

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

Last updated at Posted at 2018-06-17

環境

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

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

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

概要

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

marker.png
画像は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」を選択します。

marker2.png

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

marker3.png

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

marker4.png

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

vis_move2.gif

参考

ROSwiki Visualization marker

目次ページへのリンク

ROS講座の目次へのリンク

18
12
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
18
12