PythonでROSのRvizに基本図形を描画する簡単な方法

概要

PythonでROSのRvizに基本図形を描画する方法を記載する。
C++は以下のROSチュートリアルが参考になるが、Pythonで記載された例がないので、私のような「どうしてもPythonで書きたい・・・」という人のために、サンプルを記載しておく。
各項目の説明は、以下のC＋＋版を参考のこと。

ja/rviz/Tutorials/Markers: Basic Shapes - ROS Wiki

プログラム

設定項目は多いが、やっていることはごくシンプル。
marker_data.poseを任意の値にすることで、いろいろなことができるはず。

import rospy
from visualization_msgs.msg import Marker

rospy.init_node("marker_pub")

pub = rospy.Publisher("arrow_pub", Marker, queue_size = 10)
rate = rospy.Rate(25)

w=0

while not rospy.is_shutdown():
    marker_data = Marker()
    marker_data.header.frame_id = "map"
    marker_data.header.stamp = rospy.Time.now()

    marker_data.ns = "basic_shapes"
    marker_data.id = 0

    marker_data.action = Marker.ADD

    marker_data.pose.position.x = 0.0
    marker_data.pose.position.y = 0.0
    marker_data.pose.position.z = 0.0

    marker_data.pose.orientation.x=0.0
    marker_data.pose.orientation.y=0.0
    marker_data.pose.orientation.z=1.0
    marker_data.pose.orientation.w=0.0

    marker_data.color.r = 1.0
    marker_data.color.g = 0.0
    marker_data.color.b = 0.0
    marker_data.color.a = 1.0

    marker_data.scale.x = 1
    marker_data.scale.y = 0.1
    marker_data.scale.z = 0.1

    marker_data.lifetime = rospy.Duration()

    marker_data.type = 0

    pub.publish(marker_data)

    rate.sleep()

また、 marker_data.typeを変えることで、図形の形も変更可能なので、用途に合わせて使用する。

uint8 ARROW=0
uint8 CUBE=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 LINE_STRIP=4
uint8 LINE_LIST=5
uint8 CUBE_LIST=6
uint8 SPHERE_LIST=7
uint8 POINTS=8

動作確認

rvizを起動して、
Fixed Frameをmapに変更
add -> By topic より /arrow_pub Markerを選択
し、OKを押すと以下のように赤の矢印が表示される。